diff --git a/corelib/include/rtabmap/core/PDALWriter.h b/corelib/include/rtabmap/core/PDALWriter.h
index acbddd5414..a4ed246e6f 100644
--- a/corelib/include/rtabmap/core/PDALWriter.h
+++ b/corelib/include/rtabmap/core/PDALWriter.h
@@ -35,11 +35,11 @@ namespace rtabmap {
 
 std::string getPDALSupportedWriters();
 
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud);
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud);
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud);
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud);
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud,          const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud,       const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud,         const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
+int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud,   const std::vector<int> & cameraIds = std::vector<int>(), bool binary = false);
 
 }
 
diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h
index 5d8b5f13bf..6b9309223c 100644
--- a/corelib/include/rtabmap/core/util3d.h
+++ b/corelib/include/rtabmap/core/util3d.h
@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/SensorData.h>
 #include <rtabmap/core/Parameters.h>
 #include <opencv2/core/core.hpp>
+#include <rtabmap/core/ProgressState.h>
 #include <map>
 #include <list>
 
@@ -300,6 +301,33 @@ void RTABMAP_EXP fillProjectedCloudHoles(
 		bool verticalDirection,
 		bool fillToBorder);
 
+/**
+ * For each point, return pixel of the best camera (NodeID->CameraIndex)
+ * looking at it based on the policy and parameters
+ */
+std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_EXP projectCloudToCameras (
+		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
+		const std::map<int, Transform> & cameraPoses,
+		const std::map<int, std::vector<CameraModel> > & cameraModels,
+		float maxDistance = 0.0f,
+		float maxAngle = 0.0f,
+		const std::vector<float> & roiRatios = std::vector<float>(),
+		bool distanceToCamPolicy = false,
+		const ProgressState * state = 0);
+/**
+ * For each point, return pixel of the best camera (NodeID->CameraIndex)
+ * looking at it based on the policy and parameters
+ */
+std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > RTABMAP_EXP projectCloudToCameras (
+		const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
+		const std::map<int, Transform> & cameraPoses,
+		const std::map<int, std::vector<CameraModel> > & cameraModels,
+		float maxDistance = 0.0f,
+		float maxAngle = 0.0f,
+		const std::vector<float> & roiRatios = std::vector<float>(),
+		bool distanceToCamPolicy = false,
+		const ProgressState * state = 0);
+
 bool RTABMAP_EXP isFinite(const cv::Point3f & pt);
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP concatenateClouds(
diff --git a/corelib/src/PDALWriter.cpp b/corelib/src/PDALWriter.cpp
index 4cecbd6a49..de9381363c 100644
--- a/corelib/src/PDALWriter.cpp
+++ b/corelib/src/PDALWriter.cpp
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <pdal/io/BufferReader.hpp>
 #include <pdal/StageFactory.hpp>
 #include <pdal/PluginManager.hpp>
@@ -72,14 +73,31 @@ std::string getPDALSupportedWriters()
 	return output;
 }
 
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud)
+int savePDALFile(const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZ> & cloud,
+		const std::vector<int> & cameraIds,
+		bool binary)
 {
+	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
+			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+
 	pdal::PointTable table;
 
-	table.layout()->registerDims({
-		pdal::Dimension::Id::X,
-		pdal::Dimension::Id::Y,
-		pdal::Dimension::Id::Z});
+	if(!cameraIds.empty())
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::PointSourceId});
+	}
+	else
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z});
+	}
 	pdal::BufferReader bufferReader;
 
 	pdal::PointViewPtr view(new pdal::PointView(table));
@@ -88,15 +106,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 		view->setField(pdal::Dimension::Id::X, i, cloud.at(i).x);
 		view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y);
 		view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z);
+		if(!cameraIds.empty())
+		{
+			view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
+		}
 	}
 	bufferReader.addView(view);
 
 	pdal::StageFactory factory;
-	pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath));
+	std::string ext = UFile::getExtension(filePath);
+	pdal::Stage *writer = factory.createStage("writers." + ext);
 	if(writer)
 	{
 		pdal::Options writerOps;
 		writerOps.add("filename", filePath);
+		if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
+		if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
 
 		writer->setOptions(writerOps);
 		writer->setInput(bufferReader);
@@ -115,17 +140,37 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 	return 0; //success
 }
 
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud)
+int savePDALFile(const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
+		const std::vector<int> & cameraIds,
+		bool binary)
 {
+	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
+			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+
 	pdal::PointTable table;
 
-	table.layout()->registerDims({
-		pdal::Dimension::Id::X,
-		pdal::Dimension::Id::Y,
-		pdal::Dimension::Id::Z,
-		pdal::Dimension::Id::Red,
-		pdal::Dimension::Id::Green,
-		pdal::Dimension::Id::Blue});
+	if(!cameraIds.empty())
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Red,
+			pdal::Dimension::Id::Green,
+			pdal::Dimension::Id::Blue,
+			pdal::Dimension::Id::PointSourceId});
+	}
+	else
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Red,
+			pdal::Dimension::Id::Green,
+			pdal::Dimension::Id::Blue});
+	}
 	pdal::BufferReader bufferReader;
 
 	pdal::PointViewPtr view(new pdal::PointView(table));
@@ -137,15 +182,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 		view->setField(pdal::Dimension::Id::Red, i, cloud.at(i).r);
 		view->setField(pdal::Dimension::Id::Green, i, cloud.at(i).g);
 		view->setField(pdal::Dimension::Id::Blue, i, cloud.at(i).b);
+		if(!cameraIds.empty())
+		{
+			view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
+		}
 	}
 	bufferReader.addView(view);
 
 	pdal::StageFactory factory;
-	pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath));
+	std::string ext = UFile::getExtension(filePath);
+	pdal::Stage *writer = factory.createStage("writers." + ext);
 	if(writer)
 	{
 		pdal::Options writerOps;
 		writerOps.add("filename", filePath);
+		if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
+		if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
 
 		writer->setOptions(writerOps);
 		writer->setInput(bufferReader);
@@ -164,20 +216,43 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 	return 0; //success
 }
 
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud)
+int savePDALFile(const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
+		const std::vector<int> & cameraIds,
+		bool binary)
 {
+	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
+			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+
 	pdal::PointTable table;
 
-	table.layout()->registerDims({
-		pdal::Dimension::Id::X,
-		pdal::Dimension::Id::Y,
-		pdal::Dimension::Id::Z,
-		pdal::Dimension::Id::Red,
-		pdal::Dimension::Id::Green,
-		pdal::Dimension::Id::Blue,
-		pdal::Dimension::Id::NormalX,
-		pdal::Dimension::Id::NormalY,
-		pdal::Dimension::Id::NormalZ});
+	if(!cameraIds.empty())
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Red,
+			pdal::Dimension::Id::Green,
+			pdal::Dimension::Id::Blue,
+			pdal::Dimension::Id::NormalX,
+			pdal::Dimension::Id::NormalY,
+			pdal::Dimension::Id::NormalZ,
+			pdal::Dimension::Id::PointSourceId});
+	}
+	else
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Red,
+			pdal::Dimension::Id::Green,
+			pdal::Dimension::Id::Blue,
+			pdal::Dimension::Id::NormalX,
+			pdal::Dimension::Id::NormalY,
+			pdal::Dimension::Id::NormalZ});
+	}
 	pdal::BufferReader bufferReader;
 
 	pdal::PointViewPtr view(new pdal::PointView(table));
@@ -192,15 +267,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 		view->setField(pdal::Dimension::Id::NormalX, i, cloud.at(i).normal_x);
 		view->setField(pdal::Dimension::Id::NormalY, i, cloud.at(i).normal_y);
 		view->setField(pdal::Dimension::Id::NormalZ, i, cloud.at(i).normal_z);
+		if(!cameraIds.empty())
+		{
+			view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
+		}
 	}
 	bufferReader.addView(view);
 
 	pdal::StageFactory factory;
-	pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath));
+	std::string ext = UFile::getExtension(filePath);
+	pdal::Stage *writer = factory.createStage("writers." + ext);
 	if(writer)
 	{
 		pdal::Options writerOps;
 		writerOps.add("filename", filePath);
+		if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
+		if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
 
 		writer->setOptions(writerOps);
 		writer->setInput(bufferReader);
@@ -219,15 +301,33 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 	return 0; //success
 }
 
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud)
+int savePDALFile(const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZI> & cloud,
+		const std::vector<int> & cameraIds,
+		bool binary)
 {
+	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
+			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+
 	pdal::PointTable table;
 
-	table.layout()->registerDims({
-		pdal::Dimension::Id::X,
-		pdal::Dimension::Id::Y,
-		pdal::Dimension::Id::Z,
-		pdal::Dimension::Id::Intensity});
+	if(!cameraIds.empty())
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Intensity,
+			pdal::Dimension::Id::PointSourceId});
+	}
+	else
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Intensity});
+	}
 	pdal::BufferReader bufferReader;
 
 	pdal::PointViewPtr view(new pdal::PointView(table));
@@ -237,15 +337,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 		view->setField(pdal::Dimension::Id::Y, i, cloud.at(i).y);
 		view->setField(pdal::Dimension::Id::Z, i, cloud.at(i).z);
 		view->setField(pdal::Dimension::Id::Intensity, i, (unsigned short)cloud.at(i).intensity);
+		if(!cameraIds.empty())
+		{
+			view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
+		}
 	}
 	bufferReader.addView(view);
 
 	pdal::StageFactory factory;
-	pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath));
+	std::string ext = UFile::getExtension(filePath);
+	pdal::Stage *writer = factory.createStage("writers." + ext);
 	if(writer)
 	{
 		pdal::Options writerOps;
 		writerOps.add("filename", filePath);
+		if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
+		if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
 
 		writer->setOptions(writerOps);
 		writer->setInput(bufferReader);
@@ -264,18 +371,39 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 	return 0; //success
 }
 
-int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZINormal> & cloud)
+int savePDALFile(const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
+		const std::vector<int> & cameraIds,
+		bool binary)
 {
+	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
+			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+
 	pdal::PointTable table;
 
-	table.layout()->registerDims({
-		pdal::Dimension::Id::X,
-		pdal::Dimension::Id::Y,
-		pdal::Dimension::Id::Z,
-		pdal::Dimension::Id::Intensity,
-		pdal::Dimension::Id::NormalX,
-		pdal::Dimension::Id::NormalY,
-		pdal::Dimension::Id::NormalZ});
+	if(!cameraIds.empty())
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Intensity,
+			pdal::Dimension::Id::NormalX,
+			pdal::Dimension::Id::NormalY,
+			pdal::Dimension::Id::NormalZ,
+			pdal::Dimension::Id::PointSourceId});
+	}
+	else
+	{
+		table.layout()->registerDims({
+			pdal::Dimension::Id::X,
+			pdal::Dimension::Id::Y,
+			pdal::Dimension::Id::Z,
+			pdal::Dimension::Id::Intensity,
+			pdal::Dimension::Id::NormalX,
+			pdal::Dimension::Id::NormalY,
+			pdal::Dimension::Id::NormalZ});
+	}
 	pdal::BufferReader bufferReader;
 
 	pdal::PointViewPtr view(new pdal::PointView(table));
@@ -288,15 +416,22 @@ int savePDALFile(const std::string & filePath, const pcl::PointCloud<pcl::PointX
 		view->setField(pdal::Dimension::Id::NormalX, i, cloud.at(i).normal_x);
 		view->setField(pdal::Dimension::Id::NormalY, i, cloud.at(i).normal_y);
 		view->setField(pdal::Dimension::Id::NormalZ, i, cloud.at(i).normal_z);
+		if(!cameraIds.empty())
+		{
+			view->setField(pdal::Dimension::Id::PointSourceId, i, cameraIds.at(i));
+		}
 	}
 	bufferReader.addView(view);
 
 	pdal::StageFactory factory;
-	pdal::Stage *writer = factory.createStage("writers." + UFile::getExtension(filePath));
+	std::string ext = UFile::getExtension(filePath);
+	pdal::Stage *writer = factory.createStage("writers." + ext);
 	if(writer)
 	{
 		pdal::Options writerOps;
 		writerOps.add("filename", filePath);
+		if(ext.compare("ply")==0) writerOps.add("storage_mode", binary?"little endian":"ascii"); // PLY
+		if(ext.compare("pcd")==0) writerOps.add("compression", binary?"binary":"ascii"); // PCD
 
 		writer->setOptions(writerOps);
 		writer->setInput(bufferReader);
diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp
index a18db4aa90..922a749740 100644
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -1235,6 +1235,7 @@ bool Rtabmap::process(
 	bool smallDisplacement = false;
 	bool tooFastMovement = false;
 	std::list<int> signaturesRemoved;
+	bool neighborLinkRefined = false;
 	if(_rgbdSlamMode)
 	{
 		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(0,0));
@@ -1363,7 +1364,8 @@ bool Rtabmap::process(
 							_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
 						}
 					}
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
+					neighborLinkRefined = !t.isNull();
+					statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(),neighborLinkRefined?1.0f:0);
 					statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
 					statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_inliers_ratio(), info.icpInliersRatio);
 					statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_rotation(), info.icpRotation);
@@ -2686,7 +2688,7 @@ bool Rtabmap::process(
 	     lastProximitySpaceClosureId>0 || // can be different map of the current one
 	     statistics_.reducedIds().size() ||
 		 (signature->hasLink(signature->id(), Link::kPosePrior) && !_graphOptimizer->priorsIgnored()) || // prior edge
-		 (signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f && !_memory->isOdomGravityUsed()) || // gravity edge
+		 (signature->hasLink(signature->id(), Link::kGravity) && _graphOptimizer->gravitySigma()>0.0f && (!_memory->isOdomGravityUsed() || neighborLinkRefined)) || // gravity edge
 	     proximityDetectionsInTimeFound>0 ||
 		 landmarkDetected!=0 ||
 		 signaturesRetrieved.size()) // can be different map of the current one
diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp
index 38fe9c2cb5..632eb10483 100644
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/io/pcd_io.h>
 #include <pcl/io/ply_io.h>
 #include <pcl/common/transforms.h>
+#include <pcl/common/common.h>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/imgproc/types_c.h>
 
@@ -3015,52 +3016,6 @@ cv::Mat projectCloudToCamera(
 	{
 		UERROR("field map pcl::pointXYZ not found!");
 	}
-/*
-	int count = 0;
-	for(int i=0; i<(int)laserScan->size(); ++i)
-	{
-		// Get 3D from laser scan
-		pcl::PointXYZ ptScan = laserScan->at(i);
-		ptScan = util3d::transformPoint(ptScan, t);
-
-		// re-project in camera frame
-		float z = ptScan.z;
-		bool set = false;
-		if(z > 0.0f)
-		{
-			float invZ = 1.0f/z;
-			float dx = (fx*ptScan.x)*invZ + cx;
-			float dy = (fy*ptScan.y)*invZ + cy;
-			int dx_low = dx;
-			int dy_low = dy;
-			int dx_high = dx + 0.5f;
-			int dy_high = dy + 0.5f;
-			if(uIsInBounds(dx_low, 0, registered.cols) && uIsInBounds(dy_low, 0, registered.rows))
-			{
-				set = true;
-				float &zReg = registered.at<float>(dy_low, dx_low);
-				if(zReg == 0 || z < zReg)
-				{
-					zReg = z;
-				}
-			}
-			if((dx_low != dx_high || dy_low != dy_high) &&
-				uIsInBounds(dx_high, 0, registered.cols) && uIsInBounds(dy_high, 0, registered.rows))
-			{
-				set = true;
-				float &zReg = registered.at<float>(dy_high, dx_high);
-				if(zReg == 0 || z < zReg)
-				{
-					zReg = z;
-				}
-			}
-		}
-		if(set)
-		{
-			count++;
-		}
-	}
-	*/
 	UDEBUG("Points in camera=%d/%d", count, (int)laserScan->data.size());
 
 	return registered;
@@ -3149,6 +3104,275 @@ void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection,
 	}
 }
 
+struct ProjectionInfo {
+	int nodeID;
+	int cameraIndex;
+	pcl::PointXY uv;
+	float distance;
+};
+
+/**
+ * For each point, return pixel of the best camera (NodeID->CameraIndex)
+ * looking at it based on the policy and parameters
+ */
+template<class PointT>
+std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamerasImpl (
+		const typename pcl::PointCloud<PointT> & cloud,
+		const std::map<int, Transform> & cameraPoses,
+		const std::map<int, std::vector<CameraModel> > & cameraModels,
+		float maxDistance,
+		float maxAngle,
+		const std::vector<float> & roiRatios,
+		bool distanceToCamPolicy,
+		const ProgressState * state)
+{
+	std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
+
+	if (cloud.empty() || cameraPoses.empty() || cameraModels.empty())
+		return pointToPixel;
+
+	std::string msg = uFormat("Computing visible points per cam (%d points, %d cams)", (int)cloud.size(), (int)cameraPoses.size());
+	UINFO(msg.c_str());
+	if(state && !state->callback(msg))
+	{
+		//cancelled!
+		UWARN("Projecting to cameras cancelled!");
+		return pointToPixel;
+	}
+
+	std::vector<std::vector<ProjectionInfo> > invertedIndex(cloud.size()); // For each point: list of cameras
+	int cameraProcessed = 0;
+	for(std::map<int, Transform>::const_iterator pter = cameraPoses.lower_bound(0); pter!=cameraPoses.end(); ++pter)
+	{
+		std::map<int, std::vector<CameraModel> >::const_iterator iter=cameraModels.begin();
+		if(iter!=cameraModels.end() && !iter->second.empty())
+		{
+			for(size_t i=0; i<iter->second.size(); ++i)
+			{
+				Transform cameraTransform = (pter->second * iter->second[i].localTransform());
+				UASSERT(!cameraTransform.isNull());
+				cv::Mat cameraMatrixK = iter->second[i].K();
+				UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);
+				const cv::Size & imageSize = iter->second[i].imageSize();
+
+				float fx = cameraMatrixK.at<double>(0,0);
+				float fy = cameraMatrixK.at<double>(1,1);
+				float cx = cameraMatrixK.at<double>(0,2);
+				float cy = cameraMatrixK.at<double>(1,2);
+
+				// depth: 2 channels UINT: [depthMM, indexPt]
+				cv::Mat registered = cv::Mat::zeros(imageSize, CV_32SC2);
+				Transform t = cameraTransform.inverse();
+
+				cv::Rect roi(0,0,imageSize.width, imageSize.height);
+				if(roiRatios.size()==4)
+				{
+					roi = util2d::computeRoi(imageSize, roiRatios);
+				}
+
+				int count = 0;
+				for(size_t i=0; i<cloud.size(); ++i)
+				{
+					// Get 3D from laser scan
+					PointT ptScan = cloud.at(i);
+					ptScan = util3d::transformPoint(ptScan, t);
+
+					// re-project in camera frame
+					float z = ptScan.z;
+					bool set = false;
+					if(z > 0.0f)
+					{
+						float invZ = 1.0f/z;
+						float dx = (fx*ptScan.x)*invZ + cx;
+						float dy = (fy*ptScan.y)*invZ + cy;
+						int dx_low = dx;
+						int dy_low = dy;
+						int dx_high = dx + 0.5f;
+						int dy_high = dy + 0.5f;
+						int zMM = z * 1000;
+						if(uIsInBounds(dx_low, roi.x, roi.x+roi.width) && uIsInBounds(dy_low, roi.y, roi.y+roi.height))
+						{
+							set = true;
+							cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_low, dx_low);
+							if(zReg[0] == 0 || zMM < zReg[0])
+							{
+								zReg[0] = zMM;
+								zReg[1] = i;
+							}
+						}
+						if((dx_low != dx_high || dy_low != dy_high) &&
+							uIsInBounds(dx_high, roi.x, roi.x+roi.width) && uIsInBounds(dy_high, roi.y, roi.y+roi.height))
+						{
+							set = true;
+							cv::Vec2i &zReg = registered.at<cv::Vec2i>(dy_high, dx_high);
+							if(zReg[0] == 0 || zMM < zReg[0])
+							{
+								zReg[0] = zMM;
+								zReg[1] = i;
+							}
+						}
+					}
+					if(set)
+					{
+						count++;
+					}
+				}
+				if(count == 0)
+				{
+					registered = cv::Mat();
+					UINFO("No points projected in camera %d/%d", pter->first, i);
+				}
+				else
+				{
+					UDEBUG("%d points projected in camera %d/%d", count, pter->first, i);
+				}
+				for(int u=0; u<registered.cols; ++u)
+				{
+					for(int v=0; v<registered.rows; ++v)
+					{
+						cv::Vec2i &zReg = registered.at<cv::Vec2i>(v, u);
+						if(zReg[0] > 0)
+						{
+							ProjectionInfo info;
+							info.nodeID = pter->first;
+							info.cameraIndex = i;
+							info.uv.x = float(u)/float(imageSize.width);
+							info.uv.y = float(v)/float(imageSize.height);
+							info.distance = zReg[0]/1000.0f;
+							invertedIndex[zReg[1]].push_back(info);
+						}
+					}
+				}
+			}
+		}
+
+		msg = uFormat("Processed camera %d/%d", (int)cameraProcessed+1, (int)cameraPoses.size());
+		UINFO(msg.c_str());
+		if(state && !state->callback(msg))
+		{
+			//cancelled!
+			UWARN("Projecting to cameras cancelled!");
+			return pointToPixel;
+		}
+		++cameraProcessed;
+	}
+
+	msg = uFormat("Select best camera for %d points...", (int)cloud.size());
+	UINFO(msg.c_str());
+	if(state && !state->callback(msg))
+	{
+		//cancelled!
+		UWARN("Projecting to cameras cancelled!");
+		return pointToPixel;
+	}
+
+	pointToPixel.resize(invertedIndex.size());
+	int colorized = 0;
+
+	// For each point
+	for(size_t i=0; i<invertedIndex.size(); ++i)
+	{
+		if((i+1)%10000 == 0)
+		{
+			UDEBUG("Point %d/%d", i+1, (int)cloud.size());
+			if(state && !state->callback(uFormat("%d/%d points projected to cameras (out of %d points)", colorized, i+1, (int)cloud.size())))
+			{
+				//cancelled!
+				UWARN("Projecting to camera cancelled!");
+				pointToPixel.clear();
+				return pointToPixel;
+			}
+		}
+
+		const PointT & pt = cloud.at(i);
+		int nodeID = -1;
+		int cameraIndex = -1;
+		float smallestWeight = std::numeric_limits<float>::max();
+		pcl::PointXY uv_coords;
+		for (size_t j = 0; j<invertedIndex[i].size(); ++j)
+		{
+			const Transform & cam = cameraPoses.at(invertedIndex[i][j].nodeID);
+			Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
+			Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
+			float angleToCam = pcl::getAngle3D(normal, camDir);
+			float distanceToCam = invertedIndex[i][j].distance;
+			if(camDir.dot(normal) > 0 &&                       // is facing camera?
+				(maxAngle<=0 || angleToCam < maxAngle) &&      // is point normal perpendicular to camera?
+				(maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
+			{
+				float vx = invertedIndex[i][j].uv.x-0.5f;
+				float vy = invertedIndex[i][j].uv.y-0.5f;
+
+				float distanceToCenter = vx*vx+vy*vy;
+				float distance = distanceToCenter;
+				if(distanceToCamPolicy)
+				{
+					distance = distanceToCam;
+				}
+				if(distance <= smallestWeight)
+				{
+					nodeID = invertedIndex[i][j].nodeID;
+					cameraIndex = invertedIndex[i][j].cameraIndex;
+					smallestWeight = distance;
+					uv_coords = invertedIndex[i][j].uv;
+				}
+			}
+		}
+
+		if(nodeID>-1 && cameraIndex> -1)
+		{
+			pointToPixel[i].first.first = nodeID;
+			pointToPixel[i].first.second = cameraIndex;
+			pointToPixel[i].second = uv_coords;
+			++colorized;
+		}
+	}
+
+	UINFO("Process %d points...done! (%d [%d%%] projected in cameras)", (int)cloud.size(), colorized, colorized*100/cloud.size());
+
+	return pointToPixel;
+}
+
+std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCameras (
+		const typename pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
+		const std::map<int, Transform> & cameraPoses,
+		const std::map<int, std::vector<CameraModel> > & cameraModels,
+		float maxDistance,
+		float maxAngle,
+		const std::vector<float> & roiRatios,
+		bool distanceToCamPolicy,
+		const ProgressState * state)
+{
+	return projectCloudToCamerasImpl(cloud,
+			cameraPoses,
+			cameraModels,
+			maxDistance,
+			maxAngle,
+			roiRatios,
+			distanceToCamPolicy,
+			state);
+}
+
+std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCameras (
+		const typename pcl::PointCloud<pcl::PointXYZINormal> & cloud,
+		const std::map<int, Transform> & cameraPoses,
+		const std::map<int, std::vector<CameraModel> > & cameraModels,
+		float maxDistance,
+		float maxAngle,
+		const std::vector<float> & roiRatios,
+		bool distanceToCamPolicy,
+		const ProgressState * state)
+{
+	return projectCloudToCamerasImpl(cloud,
+			cameraPoses,
+			cameraModels,
+			maxDistance,
+			maxAngle,
+			roiRatios,
+			distanceToCamPolicy,
+			state);
+}
+
 bool isFinite(const cv::Point3f & pt)
 {
 	return uIsFinite(pt.x) && uIsFinite(pt.y) && uIsFinite(pt.z);
diff --git a/guilib/include/rtabmap/gui/ExportCloudsDialog.h b/guilib/include/rtabmap/gui/ExportCloudsDialog.h
index 7236de26bf..8faf088b9f 100644
--- a/guilib/include/rtabmap/gui/ExportCloudsDialog.h
+++ b/guilib/include/rtabmap/gui/ExportCloudsDialog.h
@@ -138,7 +138,7 @@ private Q_SLOTS:
 			const std::map<int, LaserScan> & cachedScans,
 			const ParametersMap & parameters,
 			bool & has2dScans) const;
-	void saveClouds(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, bool binaryMode = true);
+	void saveClouds(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds, bool binaryMode = true, const std::vector<std::map<int, pcl::PointXY> > & pointToPixels = std::vector<std::map<int, pcl::PointXY> >());
 	void saveMeshes(const QString & workingDirectory, const std::map<int, Transform> & poses, const std::map<int, pcl::PolygonMesh::Ptr> & meshes, bool binaryMode = true);
 	void saveTextureMeshes(const QString & workingDirectory, const std::map<int, Transform> & poses, std::map<int, pcl::TextureMesh::Ptr> & textureMeshes, const QMap<int, Signature> & cachedSignatures, const std::vector<std::map<int, pcl::PointXY> > & textureVertexToPixels);
 
diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp
index b56fb035e9..6771f7bc74 100644
--- a/guilib/src/ExportCloudsDialog.cpp
+++ b/guilib/src/ExportCloudsDialog.cpp
@@ -115,6 +115,8 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->spinBox_decimation, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_maxDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_minDepth, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_ceilingHeight, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_floorHeight, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->spinBox_decimation_scan, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_rangeMin, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_rangeMax, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
@@ -173,9 +175,23 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->checkBox_blending, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->comboBox_blendingDecimation, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
 
+	connect(_ui->checkBox_cameraProjection, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_cameraProjection, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
+	connect(_ui->lineEdit_camProjRoiRatios, SIGNAL(textChanged(const QString &)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_camProjMaxDistance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_camProjMaxAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_camProjDistanceToCamPolicy, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_camProjKeepPointsNotSeenByCameras, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->checkBox_camProjRecolorPoints, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->comboBox_camProjExportCamera, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
+#ifndef RTABMAP_PDAL
+	_ui->comboBox_camProjExportCamera->setEnabled(false);
+	_ui->label_camProjExportCamera->setEnabled(false);
+	_ui->label_camProjExportCamera->setText(_ui->label_camProjExportCamera->text() + " (PDAL dependency required)");
+#endif
+
 	connect(_ui->checkBox_meshing, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_meshing, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
-	connect(_ui->checkBox_meshing, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
 	connect(_ui->doubleSpinBox_gp3Radius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_gp3Mu, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->doubleSpinBox_meshDecimationFactor, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
@@ -313,6 +329,8 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("regenerate_decimation", _ui->spinBox_decimation->value());
 	settings.setValue("regenerate_max_depth", _ui->doubleSpinBox_maxDepth->value());
 	settings.setValue("regenerate_min_depth", _ui->doubleSpinBox_minDepth->value());
+	settings.setValue("regenerate_ceiling", _ui->doubleSpinBox_ceilingHeight->value());
+	settings.setValue("regenerate_floor", _ui->doubleSpinBox_floorHeight->value());
 	settings.setValue("regenerate_scan_decimation", _ui->spinBox_decimation_scan->value());
 	settings.setValue("regenerate_scan_max_range", _ui->doubleSpinBox_rangeMax->value());
 	settings.setValue("regenerate_scan_min_range", _ui->doubleSpinBox_rangeMin->value());
@@ -356,6 +374,15 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("gain_rgb", _ui->checkBox_gainRGB->isChecked());
 	settings.setValue("gain_full", _ui->checkBox_gainFull->isChecked());
 
+	settings.setValue("cam_proj", _ui->checkBox_cameraProjection->isChecked());
+	settings.setValue("cam_proj_roi_ratios", _ui->lineEdit_camProjRoiRatios->text());
+	settings.setValue("cam_proj_max_distance", _ui->doubleSpinBox_camProjMaxDistance->value());
+	settings.setValue("cam_proj_max_angle", _ui->doubleSpinBox_camProjMaxAngle->value());
+	settings.setValue("cam_proj_distance_policy", _ui->checkBox_camProjDistanceToCamPolicy->isChecked());
+	settings.setValue("cam_proj_keep_points", _ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked());
+	settings.setValue("cam_proj_recolor_points", _ui->checkBox_camProjRecolorPoints->isChecked());
+	settings.setValue("cam_proj_export_format", _ui->comboBox_camProjExportCamera->currentIndex());
+
 	settings.setValue("mesh", _ui->checkBox_meshing->isChecked());
 	settings.setValue("mesh_radius", _ui->doubleSpinBox_gp3Radius->value());
 	settings.setValue("mesh_mu", _ui->doubleSpinBox_gp3Mu->value());
@@ -451,6 +478,8 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->spinBox_decimation->setValue(settings.value("regenerate_decimation", _ui->spinBox_decimation->value()).toInt());
 	_ui->doubleSpinBox_maxDepth->setValue(settings.value("regenerate_max_depth", _ui->doubleSpinBox_maxDepth->value()).toDouble());
 	_ui->doubleSpinBox_minDepth->setValue(settings.value("regenerate_min_depth", _ui->doubleSpinBox_minDepth->value()).toDouble());
+	_ui->doubleSpinBox_ceilingHeight->setValue(settings.value("regenerate_ceiling", _ui->doubleSpinBox_ceilingHeight->value()).toDouble());
+	_ui->doubleSpinBox_floorHeight->setValue(settings.value("regenerate_floor", _ui->doubleSpinBox_floorHeight->value()).toDouble());
 	_ui->spinBox_decimation_scan->setValue(settings.value("regenerate_scan_decimation", _ui->spinBox_decimation_scan->value()).toInt());
 	_ui->doubleSpinBox_rangeMax->setValue(settings.value("regenerate_scan_max_range", _ui->doubleSpinBox_rangeMax->value()).toDouble());
 	_ui->doubleSpinBox_rangeMin->setValue(settings.value("regenerate_scan_min_range", _ui->doubleSpinBox_rangeMin->value()).toDouble());
@@ -497,6 +526,15 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->checkBox_gainRGB->setChecked(settings.value("gain_rgb", _ui->checkBox_gainRGB->isChecked()).toBool());
 	_ui->checkBox_gainFull->setChecked(settings.value("gain_full", _ui->checkBox_gainFull->isChecked()).toBool());
 
+	_ui->checkBox_cameraProjection->setChecked(settings.value("cam_proj", _ui->checkBox_cameraProjection->isChecked()).toBool());
+	_ui->lineEdit_camProjRoiRatios->setText(settings.value("cam_proj_roi_ratios", _ui->lineEdit_camProjRoiRatios->text()).toString());
+	_ui->doubleSpinBox_camProjMaxDistance->setValue(settings.value("cam_proj_max_distance", _ui->doubleSpinBox_camProjMaxDistance->value()).toDouble());
+	_ui->doubleSpinBox_camProjMaxAngle->setValue(settings.value("cam_proj_max_angle", _ui->doubleSpinBox_camProjMaxAngle->value()).toDouble());
+	_ui->checkBox_camProjDistanceToCamPolicy->setChecked(settings.value("cam_proj_distance_policy", _ui->checkBox_camProjDistanceToCamPolicy->isChecked()).toBool());
+	_ui->checkBox_camProjKeepPointsNotSeenByCameras->setChecked(settings.value("cam_proj_keep_points", _ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked()).toBool());
+	_ui->checkBox_camProjRecolorPoints->setChecked(settings.value("cam_proj_recolor_points", _ui->checkBox_camProjRecolorPoints->isChecked()).toBool());
+	_ui->comboBox_camProjExportCamera->setCurrentIndex(settings.value("cam_proj_export_format", _ui->comboBox_camProjExportCamera->currentIndex()).toInt());
+
 	_ui->checkBox_meshing->setChecked(settings.value("mesh", _ui->checkBox_meshing->isChecked()).toBool());
 	_ui->doubleSpinBox_gp3Radius->setValue(settings.value("mesh_radius", _ui->doubleSpinBox_gp3Radius->value()).toDouble());
 	_ui->doubleSpinBox_gp3Mu->setValue(settings.value("mesh_mu", _ui->doubleSpinBox_gp3Mu->value()).toDouble());
@@ -592,6 +630,8 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->spinBox_decimation->setValue(1);
 	_ui->doubleSpinBox_maxDepth->setValue(4);
 	_ui->doubleSpinBox_minDepth->setValue(0);
+	_ui->doubleSpinBox_ceilingHeight->setValue(0);
+	_ui->doubleSpinBox_floorHeight->setValue(0);
 	_ui->spinBox_decimation_scan->setValue(1);
 	_ui->doubleSpinBox_rangeMax->setValue(0);
 	_ui->doubleSpinBox_rangeMin->setValue(0);
@@ -635,6 +675,15 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->checkBox_gainRGB->setChecked(true);
 	_ui->checkBox_gainFull->setChecked(false);
 
+	_ui->checkBox_cameraProjection->setChecked(false);
+	_ui->lineEdit_camProjRoiRatios->setText("0.0 0.0 0.0 0.0");
+	_ui->doubleSpinBox_camProjMaxDistance->setValue(0);
+	_ui->doubleSpinBox_camProjMaxAngle->setValue(0);
+	_ui->checkBox_camProjDistanceToCamPolicy->setChecked(true);
+	_ui->checkBox_camProjKeepPointsNotSeenByCameras->setChecked(false);
+	_ui->checkBox_camProjRecolorPoints->setChecked(true);
+	_ui->comboBox_camProjExportCamera->setCurrentIndex(0);
+
 	_ui->checkBox_meshing->setChecked(false);
 	_ui->doubleSpinBox_gp3Radius->setValue(0.2);
 	_ui->doubleSpinBox_gp3Mu->setValue(2.5);
@@ -778,11 +827,15 @@ void ExportCloudsDialog::updateReconstructionFlavor()
 	_ui->checkBox_gainCompensation->setVisible(_ui->checkBox_gainCompensation->isEnabled());
 	_ui->label_gainCompensation->setVisible(_ui->checkBox_gainCompensation->isEnabled());
 
+	_ui->checkBox_cameraProjection->setEnabled(_ui->checkBox_assemble->isChecked() && !_ui->checkBox_meshing->isChecked());
+	_ui->label_cameraProjection->setEnabled(_ui->checkBox_cameraProjection->isEnabled());
+
 	_ui->groupBox_regenerate->setVisible(_ui->checkBox_regenerate->isChecked() && _ui->checkBox_fromDepth->isChecked());
 	_ui->groupBox_regenerateScans->setVisible(_ui->checkBox_regenerate->isChecked() && !_ui->checkBox_fromDepth->isChecked());
 	_ui->groupBox_bilateral->setVisible(_ui->checkBox_bilateral->isChecked());
 	_ui->groupBox_filtering->setVisible(_ui->checkBox_filtering->isChecked());
 	_ui->groupBox_gain->setVisible(_ui->checkBox_gainCompensation->isEnabled() && _ui->checkBox_gainCompensation->isChecked());
+	_ui->groupBox_cameraProjection->setVisible(_ui->checkBox_cameraProjection->isEnabled() && _ui->checkBox_cameraProjection->isChecked());
 	_ui->groupBox_mls->setVisible(_ui->checkBox_smoothing->isEnabled() && _ui->checkBox_smoothing->isChecked());
 	_ui->groupBox_meshing->setVisible(_ui->checkBox_meshing->isChecked());
 	_ui->groupBox_subtraction->setVisible(_ui->checkBox_subtraction->isChecked());
@@ -952,7 +1005,7 @@ void ExportCloudsDialog::exportClouds(
 		}
 		else
 		{
-			saveClouds(workingDirectory, poses, clouds, _ui->checkBox_binary->isChecked());
+			saveClouds(workingDirectory, poses, clouds, _ui->checkBox_binary->isChecked(), textureVertexToPixels);
 		}
 	}
 	else if(!_canceled)
@@ -1234,16 +1287,13 @@ void ExportCloudsDialog::viewClouds(
 				_progressDialog->appendText(tr("Viewing the cloud %1 (%2 points)...").arg(iter->first).arg(iter->second->size()));
 				_progressDialog->incrementStep();
 
-				QColor color = Qt::gray;
-				int mapId = uValue(mapIds, iter->first, -1);
-				if(mapId >= 0)
+				if(!_ui->checkBox_fromDepth->isChecked() &&
+					!(_ui->checkBox_cameraProjection->isEnabled() &&
+					_ui->checkBox_cameraProjection->isChecked() &&
+					_ui->checkBox_camProjRecolorPoints->isChecked() &&
+					clouds.size()==1 && clouds.begin()->first==0))
 				{
-					color = (Qt::GlobalColor)(mapId % 12 + 7 );
-				}
-
-				if(!_ui->checkBox_fromDepth->isChecked())
-				{
-					// When laser scans are exported, convert RGB to Intensity
+					// When laser scans are exported (and camera RGB was not applied), convert RGB to Intensity
 					if(_ui->spinBox_normalKSearch->value()<=0 && _ui->doubleSpinBox_normalRadiusSearch->value()<=0.0)
 					{
 						// remove normals if not used
@@ -1709,6 +1759,11 @@ bool ExportCloudsDialog::getExportedClouds(
 			if(!_ui->checkBox_fromDepth->isChecked() && !has2dScans &&
 					(_ui->spinBox_normalKSearch->value()>0 || _ui->doubleSpinBox_normalRadiusSearch->value()>0.0))
 			{
+				_progressDialog->appendText(tr("Computing normals (%1 points, K=%2, radius=%3 m)...")
+										.arg(assembledCloud->size())
+										.arg(_ui->spinBox_normalKSearch->value())
+										.arg(_ui->doubleSpinBox_normalRadiusSearch->value()));
+
 				// recompute normals
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithoutNormals(new pcl::PointCloud<pcl::PointXYZ>);
 				pcl::copyPointCloud(*assembledCloud, *cloudWithoutNormals);
@@ -2444,6 +2499,217 @@ bool ExportCloudsDialog::getExportedClouds(
 			_progressDialog->appendText(msg.c_str(), Qt::darkYellow);
 			UWARN(msg.c_str());
 		}
+		else if(_ui->checkBox_cameraProjection->isEnabled() &&
+				_ui->checkBox_cameraProjection->isChecked() &&
+				cloudsWithNormals.size()==1 && cloudsWithNormals.begin()->first==0) // only for assembled point cloud
+		{
+			_progressDialog->appendText(tr("Camera projection..."));
+			QApplication::processEvents();
+			uSleep(100);
+			QApplication::processEvents();
+
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledCloud = cloudsWithNormals.begin()->second;
+
+			std::map<int, Transform> cameraPoses;
+			std::map<int, std::vector<CameraModel> > cameraModels;
+			for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(0); iter!=poses.end(); ++iter)
+			{
+				std::vector<CameraModel> models;
+				StereoCameraModel stereoModel;
+				if(cachedSignatures.contains(iter->first))
+				{
+					const SensorData & data = cachedSignatures.find(iter->first)->sensorData();
+					models = data.cameraModels();
+					stereoModel = data.stereoCameraModel();
+				}
+				else if(_dbDriver)
+				{
+					_dbDriver->getCalibration(iter->first, models, stereoModel);
+				}
+
+				if(stereoModel.isValidForProjection())
+				{
+					models.clear();
+					models.push_back(stereoModel.left());
+				}
+				else if(models.size() == 0 || !models[0].isValidForProjection())
+				{
+					models.clear();
+				}
+
+				if(!models.empty() && models[0].imageWidth() != 0 && models[0].imageHeight() != 0)
+				{
+					cameraPoses.insert(std::make_pair(iter->first, iter->second));
+					cameraModels.insert(std::make_pair(iter->first, models));
+				}
+				else
+				{
+					UWARN("%d node has invalid camera models, ignoring it.", iter->first);
+				}
+			}
+			if(!cameraPoses.empty())
+			{
+				TexturingState texturingState(_progressDialog, true);
+				_progressDialog->setMaximumSteps(_progressDialog->maximumSteps() + assembledCloud->size()/10000+1 + cameraPoses.size());
+				std::vector<float> roiRatios;
+				QStringList strings = _ui->lineEdit_camProjRoiRatios->text().split(' ');
+				if(!_ui->lineEdit_meshingTextureRoiRatios->text().isEmpty())
+				{
+					if(strings.size()==4)
+					{
+						roiRatios.resize(4);
+						roiRatios[0]=strings[0].toDouble();
+						roiRatios[1]=strings[1].toDouble();
+						roiRatios[2]=strings[2].toDouble();
+						roiRatios[3]=strings[3].toDouble();
+						if(!(roiRatios[0]>=0.0f && roiRatios[0]<=1.0f &&
+							roiRatios[1]>=0.0f && roiRatios[1]<=1.0f &&
+							roiRatios[2]>=0.0f && roiRatios[2]<=1.0f &&
+							roiRatios[3]>=0.0f && roiRatios[3]<=1.0f))
+						{
+							roiRatios.clear();
+						}
+					}
+					if(roiRatios.empty())
+					{
+						QString msg = tr("Wrong ROI format. Region of Interest (ROI) must "
+								"have 4 values [left right top bottom] between 0 and 1 "
+								"separated by space (%1), ignoring it for projecting cameras...")
+										.arg(_ui->lineEdit_camProjRoiRatios->text());
+						UWARN(msg.toStdString().c_str());
+						_progressDialog->appendText(msg, Qt::darkYellow);
+						_progressDialog->setAutoClose(false);
+					}
+				}
+				std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
+				pointToPixel = util3d::projectCloudToCameras(
+						*assembledCloud,
+						cameraPoses,
+						cameraModels,
+						_ui->doubleSpinBox_camProjMaxDistance->value(),
+						_ui->doubleSpinBox_camProjMaxAngle->value()*M_PI/180.0,
+						roiRatios,
+						_ui->checkBox_camProjDistanceToCamPolicy->isChecked(),
+						&texturingState);
+
+				// color the cloud
+				UASSERT(pointToPixel.empty() || pointToPixel.size() == assembledCloud->size());
+				QMap<int, cv::Mat> cachedImages;
+				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledCloudValidPoints;
+				if(!_ui->checkBox_camProjKeepPointsNotSeenByCameras->isChecked())
+				{
+					assembledCloudValidPoints.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
+					assembledCloudValidPoints->resize(assembledCloud->size());
+				}
+				if(_ui->comboBox_camProjExportCamera->isEnabled() &&
+				   _ui->comboBox_camProjExportCamera->currentIndex()>0)
+				{
+					textureVertexToPixels.resize(assembledCloud->size());
+				}
+				int oi=0;
+				for(size_t i=0; i<pointToPixel.size(); ++i)
+				{
+					pcl::PointXYZRGBNormal & pt = assembledCloud->at(i);
+					if(_ui->checkBox_camProjRecolorPoints->isChecked() && !_ui->checkBox_fromDepth->isChecked())
+					{
+						pt.r = 255;
+						pt.g = 0;
+						pt.b = 0;
+						pt.a = 255;
+					}
+					int nodeID = pointToPixel[i].first.first;
+					int cameraIndex = pointToPixel[i].first.second;
+					if(nodeID>0 && cameraIndex>=0)
+					{
+						if(_ui->checkBox_camProjRecolorPoints->isChecked())
+						{
+							cv::Mat image;
+							if(cachedImages.contains(nodeID))
+							{
+								image = cachedImages.value(nodeID);
+							}
+							else if(cachedSignatures.contains(nodeID) && !cachedSignatures.value(nodeID).sensorData().imageCompressed().empty())
+							{
+								cachedSignatures.value(nodeID).sensorData().uncompressDataConst(&image, 0);
+								cachedImages.insert(nodeID, image);
+							}
+							else if(_dbDriver)
+							{
+								SensorData data;
+								_dbDriver->getNodeData(nodeID, data, true, false, false, false);
+								data.uncompressDataConst(&image, 0);
+								cachedImages.insert(nodeID, image);
+							}
+
+							if(!image.empty())
+							{
+								int subImageWidth = image.cols / cameraModels.at(nodeID).size();
+								image = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth));
+
+
+								int x = pointToPixel[i].second.x * (float)image.cols;
+								int y = pointToPixel[i].second.y * (float)image.rows;
+								UASSERT(x>=0 && x<image.cols);
+								UASSERT(y>=0 && y<image.rows);
+
+								if(image.type()==CV_8UC3)
+								{
+									cv::Vec3b bgr = image.at<cv::Vec3b>(y, x);
+									pt.b = bgr[0];
+									pt.g = bgr[1];
+									pt.r = bgr[2];
+								}
+								else
+								{
+									UASSERT(image.type()==CV_8UC1);
+									pt.r = pt.g = pt.b = image.at<unsigned char>(pointToPixel[i].second.y * image.rows, pointToPixel[i].second.x * image.cols);
+								}
+							}
+						}
+
+						int exportedId = nodeID;
+						if(_ui->comboBox_camProjExportCamera->currentIndex() == 2)
+						{
+							exportedId = cameraIndex+1;
+						}
+						else if(_ui->comboBox_camProjExportCamera->currentIndex() == 3)
+						{
+							UASSERT_MSG(cameraIndex<10, "Exporting cam id as NodeId+CamIndex is only supported when the number of cameras per node < 10.");
+							exportedId = nodeID*10 + cameraIndex;
+						}
+
+						if(assembledCloudValidPoints.get())
+						{
+							if(!textureVertexToPixels.empty())
+							{
+								textureVertexToPixels[oi].insert(std::make_pair(exportedId, pointToPixel[i].second));
+							}
+							assembledCloudValidPoints->at(oi++) = pt;
+						}
+						else if(!textureVertexToPixels.empty())
+						{
+							textureVertexToPixels[i].insert(std::make_pair(exportedId, pointToPixel[i].second));
+						}
+					}
+				}
+
+				if(assembledCloudValidPoints.get())
+				{
+					assembledCloudValidPoints->resize(oi);
+					cloudsWithNormals.begin()->second = assembledCloudValidPoints;
+
+					if(!textureVertexToPixels.empty())
+					{
+						textureVertexToPixels.resize(oi);
+					}
+				}
+
+				if(_canceled)
+				{
+					return false;
+				}
+			}
+		}
 
 		UDEBUG("");
 #ifdef RTABMAP_CPUTSDF
@@ -2933,7 +3199,10 @@ bool ExportCloudsDialog::getExportedClouds(
 							}
 							if(roiRatios.empty())
 							{
-								QString msg = tr("Wrong ROI format. Region of Interest (ROI) must have 4 values [left right top bottom] between 0 and 1 separated by space (%1), ignoring it for texturing...").arg(_ui->lineEdit_meshingTextureRoiRatios->text());
+								QString msg = tr("Wrong ROI format. Region of Interest (ROI) must have 4 "
+										"values [left right top bottom] between 0 and 1 "
+										"separated by space (%1), ignoring it for texturing...")
+												.arg(_ui->lineEdit_meshingTextureRoiRatios->text());
 								UWARN(msg.toStdString().c_str());
 								_progressDialog->appendText(msg, Qt::darkYellow);
 								_progressDialog->setAutoClose(false);
@@ -3015,8 +3284,8 @@ bool ExportCloudsDialog::getExportedClouds(
 				textureMeshes.clear();
 				textureMeshes.insert(std::make_pair(0, assembledMesh));
 			}
-
 		}
+
 		UDEBUG("");
 		return true;
 	}
@@ -3401,7 +3670,8 @@ void ExportCloudsDialog::saveClouds(
 		const QString & workingDirectory,
 		const std::map<int, Transform> & poses,
 		const std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> & clouds,
-		bool binaryMode)
+		bool binaryMode,
+		const std::vector<std::map<int, pcl::PointXY> > & pointToPixels)
 {
 	if(clouds.size() == 1)
 	{
@@ -3435,9 +3705,13 @@ void ExportCloudsDialog::saveClouds(
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBWithoutNormals;
 				pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIWithoutNormals;
 				pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals;
-				if(!_ui->checkBox_fromDepth->isChecked())
+				if(!_ui->checkBox_fromDepth->isChecked() &&
+					!(_ui->checkBox_cameraProjection->isEnabled() &&
+					_ui->checkBox_cameraProjection->isChecked() &&
+					_ui->checkBox_camProjRecolorPoints->isChecked() &&
+					clouds.size()==1 && clouds.begin()->first==0))
 				{
-					// When laser scans are exported, convert RGB to Intensity
+					// When laser scans are exported (and camera RGB was not applied), convert RGB to Intensity
 					if(_ui->spinBox_normalKSearch->value()>0 || _ui->doubleSpinBox_normalRadiusSearch->value()>0.0)
 					{
 						cloudIWithNormals.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
@@ -3505,8 +3779,11 @@ void ExportCloudsDialog::saveClouds(
 						success = pcl::io::savePCDFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0;
 					}
 				}
-				else if(QFileInfo(path).suffix() == "ply")
-				{
+#ifdef RTABMAP_PDAL
+				else if(QFileInfo(path).suffix() == "ply" && pointToPixels.empty()) {
+#else
+				else if(QFileInfo(path).suffix() == "ply") {
+#endif
 					if(cloudIWithNormals.get())
 					{
 						success = pcl::io::savePLYFile(path.toStdString(), *cloudIWithNormals, binaryMode) == 0;
@@ -3527,21 +3804,29 @@ void ExportCloudsDialog::saveClouds(
 #ifdef RTABMAP_PDAL
 				else if(!QFileInfo(path).suffix().isEmpty())
 				{
+					std::vector<int> cameraIds(pointToPixels.size(), 0);
+					for(size_t i=0;i<pointToPixels.size(); ++i)
+					{
+						if(!pointToPixels[i].empty())
+						{
+							cameraIds[i] = pointToPixels[i].begin()->first;
+						}
+					}
 					if(cloudIWithNormals.get())
 					{
-						success = savePDALFile(path.toStdString(), *cloudIWithNormals) == 0;
+						success = savePDALFile(path.toStdString(), *cloudIWithNormals, cameraIds, binaryMode) == 0;
 					}
 					else if(cloudIWithoutNormals.get())
 					{
-						success = savePDALFile(path.toStdString(), *cloudIWithoutNormals) == 0;
+						success = savePDALFile(path.toStdString(), *cloudIWithoutNormals, cameraIds, binaryMode) == 0;
 					}
 					else if(cloudRGBWithoutNormals.get())
 					{
-						success = savePDALFile(path.toStdString(), *cloudRGBWithoutNormals) == 0;
+						success = savePDALFile(path.toStdString(), *cloudRGBWithoutNormals, cameraIds, binaryMode) == 0;
 					}
 					else
 					{
-						success = savePDALFile(path.toStdString(), *clouds.begin()->second) == 0;
+						success = savePDALFile(path.toStdString(), *clouds.begin()->second, cameraIds, binaryMode) == 0;
 					}
 				}
 #endif
diff --git a/guilib/src/ui/exportCloudsDialog.ui b/guilib/src/ui/exportCloudsDialog.ui
index fc4968372a..6caff34e55 100644
--- a/guilib/src/ui/exportCloudsDialog.ui
+++ b/guilib/src/ui/exportCloudsDialog.ui
@@ -23,9 +23,9 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>0</y>
+        <y>-584</y>
         <width>780</width>
-        <height>4975</height>
+        <height>5347</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -76,13 +76,6 @@
            </property>
           </widget>
          </item>
-         <item row="13" column="0">
-          <widget class="QCheckBox" name="checkBox_gainCompensation">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
          <item row="8" column="1">
           <widget class="QLabel" name="label_voxel">
            <property name="text">
@@ -103,16 +96,6 @@
            </property>
           </widget>
          </item>
-         <item row="13" column="1">
-          <widget class="QLabel" name="label_gainCompensation">
-           <property name="text">
-            <string>Gain compensation. Normalize brightness of images.</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
          <item row="5" column="1">
           <widget class="QLabel" name="label_frame">
            <property name="text">
@@ -133,16 +116,6 @@
            </property>
           </widget>
          </item>
-         <item row="14" column="1">
-          <widget class="QLabel" name="label_binaryFile_12">
-           <property name="text">
-            <string>Meshing.</string>
-           </property>
-           <property name="wordWrap">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
          <item row="12" column="0">
           <widget class="QCheckBox" name="checkBox_smoothing">
            <property name="text">
@@ -160,13 +133,6 @@
            </property>
           </widget>
          </item>
-         <item row="14" column="0">
-          <widget class="QCheckBox" name="checkBox_meshing">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
          <item row="3" column="1">
           <widget class="QLabel" name="label_binaryFile">
            <property name="text">
@@ -323,6 +289,57 @@
            </item>
           </widget>
          </item>
+         <item row="13" column="1">
+          <widget class="QLabel" name="label_gainCompensation">
+           <property name="text">
+            <string>Gain compensation. Normalize brightness of images.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="14" column="1">
+          <widget class="QLabel" name="label_cameraProjection">
+           <property name="text">
+            <string>Camera projection. This can be used to colorize point cloud created from scans and/or export camera IDs for each point of the cloud.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="13" column="0">
+          <widget class="QCheckBox" name="checkBox_gainCompensation">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="14" column="0">
+          <widget class="QCheckBox" name="checkBox_cameraProjection">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="15" column="1">
+          <widget class="QLabel" name="label_binaryFile_12">
+           <property name="text">
+            <string>Meshing.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="15" column="0">
+          <widget class="QCheckBox" name="checkBox_meshing">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
         </layout>
        </item>
        <item>
@@ -642,8 +659,11 @@
               <property name="decimals">
                <number>2</number>
               </property>
+              <property name="minimum">
+               <double>-1000.000000000000000</double>
+              </property>
               <property name="maximum">
-               <double>100.000000000000000</double>
+               <double>1000.000000000000000</double>
               </property>
               <property name="singleStep">
                <double>0.100000000000000</double>
@@ -661,8 +681,11 @@
               <property name="decimals">
                <number>2</number>
               </property>
+              <property name="minimum">
+               <double>-1000.000000000000000</double>
+              </property>
               <property name="maximum">
-               <double>100.000000000000000</double>
+               <double>1000.000000000000000</double>
               </property>
               <property name="singleStep">
                <double>0.100000000000000</double>
@@ -1475,6 +1498,185 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
          </layout>
         </widget>
        </item>
+       <item>
+        <widget class="QGroupBox" name="groupBox_cameraProjection">
+         <property name="title">
+          <string>Camera Projection</string>
+         </property>
+         <property name="checkable">
+          <bool>false</bool>
+         </property>
+         <layout class="QGridLayout" name="gridLayout_20" columnstretch="0,0,0,1">
+          <item row="3" column="1">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxAngle">
+            <property name="suffix">
+             <string> deg</string>
+            </property>
+            <property name="decimals">
+             <number>0</number>
+            </property>
+            <property name="minimum">
+             <double>0.000000000000000</double>
+            </property>
+            <property name="maximum">
+             <double>180.000000000000000</double>
+            </property>
+            <property name="singleStep">
+             <double>1.000000000000000</double>
+            </property>
+            <property name="value">
+             <double>0.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_15">
+            <property name="text">
+             <string>Keep points not seen by the cameras. These points will be set with a pure red color (255,0,0) if the cloud was created from laser scans.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="1">
+           <widget class="QCheckBox" name="checkBox_camProjDistanceToCamPolicy">
+            <property name="text">
+             <string/>
+            </property>
+           </widget>
+          </item>
+          <item row="4" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_14">
+            <property name="text">
+             <string>Distance to camera policy. The closest camera from a point is used to colorize the point. If disabled, the camera for which the point projection is the closest of the image center is used to colorize the point.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_13">
+            <property name="text">
+             <string>Maximum distance from the camera for points to be colorized by this camera (0 means infinite).</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="3" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_12">
+            <property name="text">
+             <string>Maximum angle between camera and point's normal to apply color (0 means disabled).</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="2" column="1">
+           <widget class="QDoubleSpinBox" name="doubleSpinBox_camProjMaxDistance">
+            <property name="suffix">
+             <string> m</string>
+            </property>
+            <property name="decimals">
+             <number>1</number>
+            </property>
+            <property name="minimum">
+             <double>0.000000000000000</double>
+            </property>
+            <property name="maximum">
+             <double>99.000000000000000</double>
+            </property>
+            <property name="singleStep">
+             <double>1.000000000000000</double>
+            </property>
+            <property name="value">
+             <double>0.000000000000000</double>
+            </property>
+           </widget>
+          </item>
+          <item row="5" column="1">
+           <widget class="QCheckBox" name="checkBox_camProjKeepPointsNotSeenByCameras">
+            <property name="text">
+             <string/>
+            </property>
+           </widget>
+          </item>
+          <item row="7" column="2" colspan="2">
+           <widget class="QLabel" name="label_camProjExportCamera">
+            <property name="text">
+             <string>ID format of the camera selected for each point.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="7" column="1">
+           <widget class="QComboBox" name="comboBox_camProjExportCamera">
+            <property name="toolTip">
+             <string>By Node ID: cameras of same node have same ID
+By Camera Index: of a multi-cameras setup
+By Node ID and Camera Index: NodeID*10+CameraIndex</string>
+            </property>
+            <item>
+             <property name="text">
+              <string>Disabled</string>
+             </property>
+            </item>
+            <item>
+             <property name="text">
+              <string>By Node ID</string>
+             </property>
+            </item>
+            <item>
+             <property name="text">
+              <string>By Camera Index</string>
+             </property>
+            </item>
+            <item>
+             <property name="text">
+              <string>By Node ID and Camera Index</string>
+             </property>
+            </item>
+           </widget>
+          </item>
+          <item row="1" column="1">
+           <widget class="QLineEdit" name="lineEdit_camProjRoiRatios"/>
+          </item>
+          <item row="1" column="2" colspan="2">
+           <widget class="QLabel" name="label_meshingTextureSize_17">
+            <property name="text">
+             <string>ROI ratios [left right top bottom] between 0 and 1. This can be used to ignore black borders of RGB images caused by calibration. </string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="2">
+           <widget class="QLabel" name="label_meshingTextureSize_18">
+            <property name="text">
+             <string>Recolor points from camera projection. This would be used to color laser scans with the cameras.</string>
+            </property>
+            <property name="wordWrap">
+             <bool>true</bool>
+            </property>
+           </widget>
+          </item>
+          <item row="6" column="1">
+           <widget class="QCheckBox" name="checkBox_camProjRecolorPoints">
+            <property name="text">
+             <string/>
+            </property>
+           </widget>
+          </item>
+         </layout>
+        </widget>
+       </item>
        <item>
         <widget class="QGroupBox" name="groupBox_meshing">
          <property name="title">
diff --git a/tools/Export/main.cpp b/tools/Export/main.cpp
index 1069432f30..27d730353e 100644
--- a/tools/Export/main.cpp
+++ b/tools/Export/main.cpp
@@ -31,7 +31,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_surface.h>
+#include <rtabmap/core/util2d.h>
 #include <rtabmap/core/optimizer/OptimizerG2O.h>
+#include <rtabmap/core/Graph.h>
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UFile.h>
@@ -43,19 +45,28 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/surface/poisson.h>
 #include <stdio.h>
 
+#ifdef RTABMAP_PDAL
+#include <rtabmap/core/PDALWriter.h>
+#endif
+
 using namespace rtabmap;
 
 void showUsage()
 {
 	printf("\nUsage:\n"
-			"rtabmap-exportCloud [options] database.db\n"
+			"rtabmap-export [options] database.db\n"
 			"Options:\n"
+			"    --bin                 Export PLY in binary format.\n"
+			"    --las                 Export cloud in LAS instead of PLY (PDAL dependency required).\n"
 			"    --mesh                Create a mesh.\n"
 			"    --texture             Create a mesh with texture.\n"
 			"    --texture_size  #     Texture size (default 4096).\n"
 			"    --texture_count #     Maximum textures generated (default 1).\n"
 			"    --texture_range #     Maximum camera range for texturing a polygon (default 0 meters: no limit).\n"
 			"    --texture_d2c         Distance to camera policy.\n"
+			"    --cam_projection      Camera projection on assembled cloud and export node ID on each point (in PointSourceId field).\n"
+			"    --poses               Export optimized poses in RGB-D SLAM dataset format (stamp x y z qx qy qz qw).\n"
+			"    --images              Export images.\n"
 			"    --ba                  Do global bundle adjustment before assembling the clouds.\n"
 			"    --gain          #     Gain compensation value (default 1, set 0 to disable).\n"
 			"    --gain_gray           Do gain estimation compensation on gray channel only (default RGB channels).\n"
@@ -87,6 +98,8 @@ int main(int argc, char * argv[])
 		showUsage();
 	}
 
+	bool binary = false;
+	bool las = false;
 	bool mesh = false;
 	bool texture = false;
 	bool ba = false;
@@ -109,12 +122,28 @@ int main(int argc, char * argv[])
 	bool saveInDb = false;
 	int lowBrightnessGain = 0;
 	int highBrightnessGain = 10;
+	bool camProjection = false;
+	bool exportPoses = false;
+	bool exportImages = false;
 	for(int i=1; i<argc; ++i)
 	{
 		if(std::strcmp(argv[i], "--help") == 0)
 		{
 			showUsage();
 		}
+		else if(std::strcmp(argv[i], "--bin") == 0)
+		{
+			binary = true;
+		}
+		else if(std::strcmp(argv[i], "--las") == 0)
+		{
+#ifdef RTABMAP_PDAL
+			las = true;
+#else
+			printf("\"--las\" option cannot be used because RTAB-Map is not built with PDAL support. Will export in PLY...\n");
+#endif
+
+		}
 		else if(std::strcmp(argv[i], "--mesh") == 0)
 		{
 			mesh = true;
@@ -164,6 +193,18 @@ int main(int argc, char * argv[])
 		{
 			distanceToCamPolicy = true;
 		}
+		else if(std::strcmp(argv[i], "--cam_projection") == 0)
+		{
+			camProjection = true;
+		}
+		else if(std::strcmp(argv[i], "--poses") == 0)
+		{
+			exportPoses = true;
+		}
+		else if(std::strcmp(argv[i], "--images") == 0)
+		{
+			exportImages = true;
+		}
 		else if(std::strcmp(argv[i], "--ba") == 0)
 		{
 			ba = true;
@@ -409,35 +450,45 @@ int main(int argc, char * argv[])
 	// Construct the cloud
 	printf("Create and assemble the clouds...\n");
 	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	pcl::PointCloud<pcl::PointXYZINormal>::Ptr mergedCloudsI(new pcl::PointCloud<pcl::PointXYZINormal>);
 	std::map<int, rtabmap::Transform> cameraPoses;
+	std::map<int, double> cameraStamps;
 	std::map<int, std::vector<rtabmap::CameraModel> > cameraModels;
 	std::map<int, cv::Mat> cameraDepths;
-
+	int imagesExported = 0;
 	for(std::map<int, Transform>::iterator iter=optimizedPoses.lower_bound(1); iter!=optimizedPoses.end(); ++iter)
 	{
 		Signature node = nodes.find(iter->first)->second;
 
 		// uncompress data
 		std::vector<CameraModel> models = node.sensorData().cameraModels();
+		cv::Mat rgb;
 		cv::Mat depth;
 
 		pcl::IndicesPtr indices(new std::vector<int>);
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+		pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
 		if(cloudFromScan)
 		{
 			cv::Mat tmpDepth;
 			LaserScan scan;
-			node.sensorData().uncompressData(0, texture&&!node.sensorData().depthOrRightCompressed().empty()?&tmpDepth:0, &scan);
+			node.sensorData().uncompressData(exportImages?&rgb:0, (texture||exportImages)&&!node.sensorData().depthOrRightCompressed().empty()?&tmpDepth:0, &scan);
 			if(decimation>1 || maxRange)
 			{
 				scan = util3d::commonFiltering(scan, decimation, 0, maxRange);
 			}
-			cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
+			if(scan.hasRGB())
+			{
+				cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
+			}
+			else
+			{
+				cloudI = util3d::laserScanToPointCloudI(scan, scan.localTransform());
+			}
 		}
 		else
 		{
-			cv::Mat tmpRGB;
-			node.sensorData().uncompressData(&tmpRGB, &depth);
+			node.sensorData().uncompressData(&rgb, &depth);
 			cloud = util3d::cloudRGBFromSensorData(
 					node.sensorData(),
 					decimation,      // image decimation before creating the clouds
@@ -445,11 +496,55 @@ int main(int argc, char * argv[])
 					0.0f,
 					indices.get());
 		}
+
+		if(exportImages && !rgb.empty())
+		{
+			std::string dirSuffix = (depth.type() != CV_16UC1 && depth.type() != CV_32FC1)?"rgb":"left";
+			std::string dir = outputDirectory+"/"+baseName+"_"+dirSuffix;
+			UDirectory::makeDir(dir);
+			std::string outputPath=dir+"/"+uNumber2Str(iter->first)+".jpg";
+			cv::imwrite(outputPath, rgb);
+			++imagesExported;
+			if(!depth.empty())
+			{
+				std::string ext;
+				cv::Mat depthExported = depth;
+				if(depth.type() != CV_16UC1 && depth.type() != CV_32FC1)
+				{
+					ext = ".jpg";
+					dir = outputDirectory+"/"+baseName+"_right";
+				}
+				else
+				{
+					ext = ".png";
+					dir = outputDirectory+"/"+baseName+"_depth";
+					if(depth.type() == CV_32FC1)
+					{
+						depthExported = rtabmap::util2d::cvtDepthFromFloat(depth);
+					}
+				}
+				if(!UDirectory::exists(dir))
+				{
+					UDirectory::makeDir(dir);
+				}
+
+				outputPath=dir+"/"+uNumber2Str(iter->first)+ext;
+				cv::imwrite(outputPath, depthExported);
+			}
+		}
+
 		if(voxelSize>0.0f)
 		{
-			cloud = rtabmap::util3d::voxelize(cloud, indices, voxelSize);
+			if(cloud.get() && !cloud->empty())
+				cloud = rtabmap::util3d::voxelize(cloud, indices, voxelSize);
+			else if(cloudI.get() && !cloudI->empty())
+				cloudI = rtabmap::util3d::voxelize(cloudI, indices, voxelSize);
 		}
-		cloud = rtabmap::util3d::transformPointCloud(cloud, iter->second);
+		if(cloud.get() && !cloud->empty())
+			cloud = rtabmap::util3d::transformPointCloud(cloud, iter->second);
+		else if(cloudI.get() && !cloudI->empty())
+			cloudI = rtabmap::util3d::transformPointCloud(cloudI, iter->second);
+
 
 		Eigen::Vector3f viewpoint(iter->second.x(), iter->second.y(), iter->second.z());
 		if(cloudFromScan)
@@ -457,21 +552,37 @@ int main(int argc, char * argv[])
 			Transform lidarViewpoint = iter->second * node.sensorData().laserScanRaw().localTransform();
 			viewpoint = Eigen::Vector3f(iter->second.x(),  iter->second.y(),  iter->second.z());
 		}
-		pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloud, 20, 0.0f, viewpoint);
-
-		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
-		pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
-
-		if(mergedClouds->size() == 0)
+		if(cloud.get() && !cloud->empty())
 		{
-			*mergedClouds = *cloudWithNormals;
+			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloud, 20, 0.0f, viewpoint);
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+			pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
+			if(mergedClouds->size() == 0)
+			{
+				*mergedClouds = *cloudWithNormals;
+			}
+			else
+			{
+				*mergedClouds += *cloudWithNormals;
+			}
 		}
-		else
+		else if(cloudI.get() && !cloudI->empty())
 		{
-			*mergedClouds += *cloudWithNormals;
+			pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(cloudI, 20, 0.0f, viewpoint);
+			pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals(new pcl::PointCloud<pcl::PointXYZINormal>);
+			pcl::concatenateFields(*cloudI, *normals, *cloudIWithNormals);
+			if(mergedCloudsI->size() == 0)
+			{
+				*mergedCloudsI = *cloudIWithNormals;
+			}
+			else
+			{
+				*mergedCloudsI += *cloudIWithNormals;
+			}
 		}
 
 		cameraPoses.insert(std::make_pair(iter->first, iter->second));
+		cameraStamps.insert(std::make_pair(iter->first, node.getStamp()));
 		if(!models.empty())
 		{
 			cameraModels.insert(std::make_pair(iter->first, models));
@@ -481,9 +592,12 @@ int main(int argc, char * argv[])
 			cameraDepths.insert(std::make_pair(iter->first, depth));
 		}
 	}
-	printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), (int)mergedClouds->size());
+	printf("Create and assemble the clouds... done (%fs, %d points).\n", timer.ticks(), !mergedClouds->empty()?(int)mergedClouds->size():(int)mergedCloudsI->size());
 
-	if(mergedClouds->size())
+	if(imagesExported>0)
+		printf("%d images exported!\n", imagesExported);
+
+	if(!mergedClouds->empty() || !mergedCloudsI->empty())
 	{
 		if(saveInDb)
 		{
@@ -495,31 +609,180 @@ int main(int argc, char * argv[])
 			driver->save2DMap(cv::Mat(), 0, 0, 0);
 			driver->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
 		}
+		else if(exportPoses)
+		{
+			std::string outputPath=outputDirectory+"/"+baseName+"_poses.txt";
+			rtabmap::graph::exportPoses(outputPath, 10, cameraPoses, std::multimap<int, Link>(), cameraStamps);
+			printf("Poses exported to \"%s\".\n", outputPath.c_str());
+		}
 
-		if(!(mesh || texture))
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudToExport = mergedClouds;
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIToExport = mergedCloudsI;
+		if(voxelSize>0.0f)
 		{
-			if(voxelSize>0.0f)
+			printf("Voxel grid filtering of the assembled cloud... (voxel=%f, %d points)\n", voxelSize, !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
+			if(!cloudToExport->empty())
+			{
+				cloudToExport = util3d::voxelize(cloudToExport, voxelSize);
+			}
+			else if(!cloudIToExport->empty())
 			{
-				printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", voxelSize, (int)mergedClouds->size());
-				mergedClouds = util3d::voxelize(mergedClouds, voxelSize);
+				cloudIToExport = util3d::voxelize(cloudIToExport, voxelSize);
 			}
+			printf("Voxel grid filtering of the assembled cloud.... done! (%fs, %d points)\n", timer.ticks(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
+		}
 
-			if(saveInDb)
+		std::vector<int> pointToCamId;
+		if(camProjection && !cameraPoses.empty())
+		{
+			printf("Camera projection...\n");
+			pointToCamId.resize(!cloudToExport->empty()?cloudToExport->size():cloudIToExport->size());
+			std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
+			if(!cloudToExport->empty())
 			{
-				printf("Saving in db... (%d points)\n", (int)mergedClouds->size());
-				driver->saveOptimizedMesh(util3d::laserScanFromPointCloud(*mergedClouds, Transform(), false).data());
-				printf("Saving in db... done!\n");
+				pointToPixel = util3d::projectCloudToCameras(
+						*cloudToExport,
+						cameraPoses,
+						cameraModels,
+						textureRange,
+						0,
+						std::vector<float>(),
+						distanceToCamPolicy);
 			}
-			else
+			else if(!cloudIToExport->empty())
 			{
-				std::string outputPath=outputDirectory+"/"+baseName+"_cloud.ply";
-				printf("Saving %s... (%d points)\n", outputPath.c_str(), (int)mergedClouds->size());
-				pcl::io::savePLYFile(outputPath, *mergedClouds);
-				printf("Saving %s... done!\n", outputPath.c_str());
+				pointToPixel = util3d::projectCloudToCameras(
+						*cloudIToExport,
+						cameraPoses,
+						cameraModels,
+						textureRange,
+						0,
+						std::vector<float>(),
+						distanceToCamPolicy);
+			}
+
+			// color the cloud
+			UASSERT(pointToPixel.empty() || pointToPixel.size() == pointToCamId.size());
+			std::map<int, cv::Mat> cachedImages;
+			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledCloudValidPoints(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
+			assembledCloudValidPoints->resize(pointToCamId.size());
+
+			int oi=0;
+			for(size_t i=0; i<pointToPixel.size(); ++i)
+			{
+				pcl::PointXYZRGBNormal pt;
+				if(!cloudToExport->empty())
+				{
+					pt = cloudToExport->at(i);
+				}
+				else if(!cloudIToExport->empty())
+				{
+					pt.x = cloudIToExport->at(i).x;
+					pt.y = cloudIToExport->at(i).y;
+					pt.z = cloudIToExport->at(i).z;
+					pt.normal_x = cloudIToExport->at(i).normal_x;
+					pt.normal_y = cloudIToExport->at(i).normal_y;
+					pt.normal_z = cloudIToExport->at(i).normal_z;
+				}
+				int nodeID = pointToPixel[i].first.first;
+				int cameraIndex = pointToPixel[i].first.second;
+				if(nodeID>0 && cameraIndex>=0)
+				{
+					cv::Mat image;
+					if(uContains(cachedImages, nodeID))
+					{
+						image = cachedImages.at(nodeID);
+					}
+					else if(uContains(nodes,nodeID) && !nodes.at(nodeID).sensorData().imageCompressed().empty())
+					{
+						nodes.at(nodeID).sensorData().uncompressDataConst(&image, 0);
+						cachedImages.insert(std::make_pair(nodeID, image));
+					}
+
+					if(!image.empty())
+					{
+						int subImageWidth = image.cols / cameraModels.at(nodeID).size();
+						image = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth));
+
+
+						int x = pointToPixel[i].second.x * (float)image.cols;
+						int y = pointToPixel[i].second.y * (float)image.rows;
+						UASSERT(x>=0 && x<image.cols);
+						UASSERT(y>=0 && y<image.rows);
+
+						if(image.type()==CV_8UC3)
+						{
+							cv::Vec3b bgr = image.at<cv::Vec3b>(y, x);
+							pt.b = bgr[0];
+							pt.g = bgr[1];
+							pt.r = bgr[2];
+						}
+						else
+						{
+							UASSERT(image.type()==CV_8UC1);
+							pt.r = pt.g = pt.b = image.at<unsigned char>(pointToPixel[i].second.y * image.rows, pointToPixel[i].second.x * image.cols);
+						}
+					}
+
+					int exportedId = nodeID;
+					pointToCamId[oi] = exportedId;
+					assembledCloudValidPoints->at(oi++) = pt;
+				}
+			}
+
+			assembledCloudValidPoints->resize(oi);
+			cloudToExport = assembledCloudValidPoints;
+			cloudIToExport->clear();
+			pointToCamId.resize(oi);
+
+			printf("Camera projection... done! (%fs)\n", timer.ticks());
+		}
+
+		if(saveInDb)
+		{
+			if(!(mesh || texture))
+			{
+				printf("Saving in db... (%d points)\n", !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
+				if(!cloudToExport->empty())
+					driver->saveOptimizedMesh(util3d::laserScanFromPointCloud(*cloudToExport, Transform(), false).data());
+				else if(!cloudIToExport->empty())
+					driver->saveOptimizedMesh(util3d::laserScanFromPointCloud(*cloudIToExport, Transform(), false).data());
+				printf("Saving in db... done!\n");
 			}
 		}
 		else
 		{
+			std::string ext = las?"las":"ply";
+			std::string outputPath=outputDirectory+"/"+baseName+"_cloud."+ext;
+			printf("Saving %s... (%d points)\n", outputPath.c_str(), !cloudToExport->empty()?(int)cloudToExport->size():(int)cloudIToExport->size());
+#ifdef RTABMAP_PDAL
+			if(!cloudToExport->empty())
+				savePDALFile(outputPath, *cloudToExport, pointToCamId, binary);
+			else if(!cloudIToExport->empty())
+				savePDALFile(outputPath, *cloudIToExport, pointToCamId, binary);
+#else
+			if(!pointToCamId.empty())
+			{
+				printf("Option --cam_projection is enabled but rtabmap is not built "
+						"with PDAL support, so camera IDs won't be exported in the output cloud.\n");
+			}
+			if(!cloudToExport->empty())
+				pcl::io::savePLYFile(outputPath, *cloudToExport, binary);
+			else if(!cloudIToExport->empty())
+				pcl::io::savePLYFile(outputPath, *cloudIToExport, binary);
+#endif
+			printf("Saving %s... done!\n", outputPath.c_str());
+		}
+
+		// Meshing...
+		if(mesh || texture)
+		{
+			if(!mergedCloudsI->empty())
+			{
+				pcl::copyPointCloud(*mergedCloudsI, *mergedClouds);
+				mergedCloudsI->clear();
+			}
+
 			Eigen::Vector4f min,max;
 			pcl::getMinMax3D(*mergedClouds, min, max);
 			float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]);
@@ -574,7 +837,10 @@ int main(int argc, char * argv[])
 					{
 						std::string outputPath=outputDirectory+"/"+baseName+"_mesh.ply";
 						printf("Saving %s...\n", outputPath.c_str());
-						pcl::io::savePLYFile(outputPath, *mesh);
+						if(binary)
+							pcl::io::savePLYFileBinary(outputPath, *mesh);
+						else
+							pcl::io::savePLYFile(outputPath, *mesh);
 						printf("Saving %s... done!\n", outputPath.c_str());
 					}
 				}