Export: optimized camera projection RAM usage. Added --texture_angle and --cam_projection_decimation options.

corelib/src/util3d.cpp

@@ -2823,7 +2823,13 @@ void fillProjectedCloudHoles(cv::Mat & registeredDepth, bool verticalDirection,
 	}
 }
 
-struct ProjectionInfo {
+class ProjectionInfo {
+public:
+	ProjectionInfo():
+		nodeID(-1),
+		cameraIndex(-1),
+		distance(-1)
+	{}
 	int nodeID;
 	int cameraIndex;
 	pcl::PointXY uv;
@@ -2845,6 +2851,12 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		bool distanceToCamPolicy,
 		const ProgressState * state)
 {
+	UINFO("cloud=%d points", (int)cloud.size());
+	UINFO("cameraPoses=%d", (int)cameraPoses.size());
+	UINFO("cameraModels=%d", (int)cameraModels.size());
+	UINFO("maxDistance=%f", maxDistance);
+	UINFO("maxAngle=%f", maxAngle);
+	UINFO("distanceToCamPolicy=%s", distanceToCamPolicy?"true":"false");
 	std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
 
 	if (cloud.empty() || cameraPoses.empty() || cameraModels.empty())
@@ -2859,7 +2871,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		return pointToPixel;
 	}
 
-	std::vector<std::vector<ProjectionInfo> > invertedIndex(cloud.size()); // For each point: list of cameras
+	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)
 	{
@@ -2899,7 +2911,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 					// re-project in camera frame
 					float z = ptScan.z;
 					bool set = false;
-					if(z > 0.0f)
+					if(z > 0.0f && (maxDistance<=0 || z<maxDistance))
 					{
 						float invZ = 1.0f/z;
 						float dx = (fx*ptScan.x)*invZ + cx;
@@ -2957,8 +2969,39 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 							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;
+							const Transform & cam = cameraPoses.at(info.nodeID);
-							invertedIndex[zReg[1]].push_back(info);
+							const PointT & pt = cloud.at(zReg[1]);
+							Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
+							Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
+							float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
+							float distanceToCam = zReg[0]/1000.0f;
+							if( (maxAngle<=0 || (camDir.dot(normal) > 0 && angleToCam < maxAngle)) && // is facing camera? is point normal perpendicular to camera?
+								(maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
+							{
+								float vx = info.uv.x-0.5f;
+								float vy = info.uv.y-0.5f;
+
+								float distanceToCenter = vx*vx+vy*vy;
+								float distance = distanceToCenter;
+								if(distanceToCamPolicy)
+								{
+									distance = distanceToCam;
+								}
+
+								info.distance = distance;
+
+								if(invertedIndex[zReg[1]].distance != -1.0f)
+								{
+									if(distance <= invertedIndex[zReg[1]].distance)
+									{
+										invertedIndex[zReg[1]] = info;
+									}
+								}
+								else
+								{
+									invertedIndex[zReg[1]] = info;
+								}
+							}
 						}
 					}
 				}
@@ -2991,50 +3034,14 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 	// 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)
+		if(invertedIndex[i].distance > -1.0f)
 		{
-			const Transform & cam = cameraPoses.at(invertedIndex[i][j].nodeID);
+			nodeID = invertedIndex[i].nodeID;
-			Eigen::Vector4f camDir(cam.x()-pt.x, cam.y()-pt.y, cam.z()-pt.z, 0);
+			cameraIndex = invertedIndex[i].cameraIndex;
-			Eigen::Vector4f normal(pt.normal_x, pt.normal_y, pt.normal_z, 0);
+			uv_coords = invertedIndex[i].uv;
-			float angleToCam = maxAngle<=0?0:pcl::getAngle3D(normal, camDir);
-			float distanceToCam = invertedIndex[i][j].distance;
-			if( (maxAngle<=0 || (camDir.dot(normal) > 0 && angleToCam < maxAngle)) && // is facing camera? is point normal perpendicular to camera?
-				(maxDistance<=0 || distanceToCam<maxDistance)) // is point not too far from camera?
-			{
-				float vx = 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)
@@ -3046,7 +3053,12 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
 		}
 	}
 
-	UINFO("Process %d points...done! (%d [%d%%] projected in cameras)", (int)cloud.size(), colorized, colorized*100/cloud.size());
+	msg = uFormat("Process %d points...done! (%d [%d%%] projected in cameras)", (int)cloud.size(), colorized, colorized*100/cloud.size());
+	UINFO(msg.c_str());
+	if(state)
+	{
+		state->callback(msg);
+	}
 
 	return pointToPixel;
 }

guilib/src/ExportCloudsDialog.cpp

@@ -2718,7 +2718,6 @@ bool ExportCloudsDialog::getExportedClouds(
 
 				// 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())
 				{
@@ -2730,67 +2729,84 @@ bool ExportCloudsDialog::getExportedClouds(
 				{
 					textureVertexToPixels.resize(assembledCloud->size());
 				}
-				int oi=0;
+
-				for(size_t i=0; i<pointToPixel.size(); ++i)
+				if(_ui->checkBox_camProjRecolorPoints->isChecked())
 				{
-					pcl::PointXYZRGBNormal & pt = assembledCloud->at(i);
+					int imagesDone = 1;
-					if(_ui->checkBox_camProjRecolorPoints->isChecked() && !_ui->checkBox_fromDepth->isChecked())
+					for(std::map<int, rtabmap::Transform>::iterator iter=cameraPoses.begin(); iter!=cameraPoses.end(); ++iter)
 					{
-						pt.r = 255;
+						int nodeID = iter->first;
-						pt.g = 0;
+
-						pt.b = 0;
+						cv::Mat image;
-						pt.a = 255;
+						if(cachedSignatures.contains(nodeID) && !cachedSignatures.value(nodeID).sensorData().imageCompressed().empty())
-					}
-					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;
+							cachedSignatures.value(nodeID).sensorData().uncompressDataConst(&image, 0);
-							if(cachedImages.contains(nodeID))
+						}
+						else if(_dbDriver)
+						{
+							SensorData data;
+							_dbDriver->getNodeData(nodeID, data, true, false, false, false);
+							data.uncompressDataConst(&image, 0);
+						}
+						if(!image.empty())
+						{
+							UASSERT(cameraModels.find(nodeID) != cameraModels.end());
+							int modelsSize = cameraModels.at(nodeID).size();
+							for(size_t i=0; i<pointToPixel.size(); ++i)
 							{
-								image = cachedImages.value(nodeID);
+								int cameraIndex = pointToPixel[i].first.second;
-							}
+								if(nodeID == pointToPixel[i].first.first && cameraIndex>=0)
-							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);
+									pcl::PointXYZRGBNormal & pt = assembledCloud->at(i);
-									pt.b = bgr[0];
+
-									pt.g = bgr[1];
+									int subImageWidth = image.cols / modelsSize;
-									pt.r = bgr[2];
+									cv::Mat subImage = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth));
-								}
+
-								else
+									int x = pointToPixel[i].second.x * (float)subImage.cols;
-								{
+									int y = pointToPixel[i].second.y * (float)subImage.rows;
-									UASSERT(image.type()==CV_8UC1);
+									UASSERT(x>=0 && x<subImage.cols);
-									pt.r = pt.g = pt.b = image.at<unsigned char>(pointToPixel[i].second.y * image.rows, pointToPixel[i].second.x * image.cols);
+									UASSERT(y>=0 && y<subImage.rows);
+
+									if(subImage.type()==CV_8UC3)
+									{
+										cv::Vec3b bgr = subImage.at<cv::Vec3b>(y, x);
+										pt.b = bgr[0];
+										pt.g = bgr[1];
+										pt.r = bgr[2];
+									}
+									else
+									{
+										UASSERT(subImage.type()==CV_8UC1);
+										pt.r = pt.g = pt.b = subImage.at<unsigned char>(pointToPixel[i].second.y * subImage.rows, pointToPixel[i].second.x * subImage.cols);
+									}
 								}
 							}
 						}
+						QString msg = tr("Processed %1/%2 images").arg(imagesDone++).arg(cameraPoses.size());
+						UINFO(msg.toStdString().c_str());
+						_progressDialog->appendText(msg);
+					}
+				}
 
+				pcl::IndicesPtr validIndices(new std::vector<int>(pointToPixel.size()));
+				size_t oi = 0;
+				for(size_t i=0; i<pointToPixel.size(); ++i)
+				{
+					pcl::PointXYZRGBNormal & pt = assembledCloud->at(i);
+					if(pointToPixel[i].first.first <=0)
+					{
+						if(_ui->checkBox_camProjRecolorPoints->isChecked() && !_ui->checkBox_fromDepth->isChecked())
+						{
+							pt.r = 255;
+							pt.g = 0;
+							pt.b = 0;
+							pt.a = 255;
+						}
+					}
+					else
+					{
+						int nodeID = pointToPixel[i].first.first;
+						int cameraIndex = pointToPixel[i].first.second;
 						int exportedId = nodeID;
 						if(_ui->comboBox_camProjExportCamera->currentIndex() == 2)
 						{

tools/Export/main.cpp

@@ -65,10 +65,12 @@ void showUsage()
 			"    --texture_size  #     Texture size 1024, 2048, 4096, 8192, 16384 (default 8192).\n"
 			"    --texture_count #     Maximum textures generated (default 1). Ignored by --multiband option (adjust --multiband_contrib instead).\n"
 			"    --texture_range #     Maximum camera range for texturing a polygon (default 0 meters: no limit).\n"
+			"    --texture_angle #     Maximum camera angle for texturing a polygon (default 0 deg: no limit).\n"
 			"    --texture_depth_error # Maximum depth error between reprojected mesh and depth image to texture a face (-1=disabled, 0=edge length is used, default=0).\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"
 			"    --cam_projection_keep_all  Keep not colored points from cameras (node ID will be 0 and color will be red).\n"
+			"    --cam_projection_decimation  Decimate images before projecting the points.\n"
 			"    --poses               Export optimized poses of the robot frame (e.g., base_link).\n"
 			"    --poses_camera        Export optimized poses of the camera frame (e.g., optical frame).\n"
 			"    --poses_scan          Export optimized poses of the scan frame.\n"
@@ -124,6 +126,16 @@ void showUsage()
 	exit(1);
 }
 
+class ConsoleProgessState : public ProgressState
+{
+	virtual bool callback(const std::string & msg) const
+	{
+		if(!msg.empty())
+			printf("%s\n", msg.c_str());
+		return true;
+	}
+};
+
 int main(int argc, char * argv[])
 {
 	ULogger::setType(ULogger::kTypeConsole);
@@ -154,7 +166,8 @@ int main(int argc, char * argv[])
 	int noiseMinNeighbors = 5;
 	int textureSize = 8192;
 	int textureCount = 1;
-	int textureRange = 0;
+	float textureRange = 0;
+	float textureAngle = 0;
 	float textureDepthError = 0;
 	bool distanceToCamPolicy = false;
 	bool multiband = false;
@@ -173,6 +186,7 @@ int main(int argc, char * argv[])
 	int highBrightnessGain = 10;
 	bool camProjection = false;
 	bool camProjectionKeepAll = false;
+	int cameraProjDecimation = 1;
 	bool exportPoses = false;
 	bool exportPosesCamera = false;
 	bool exportPosesScan = false;
@@ -265,7 +279,19 @@ int main(int argc, char * argv[])
 			++i;
 			if(i<argc-1)
 			{
-				textureRange = uStr2Int(argv[i]);
+				textureRange = uStr2Float(argv[i]);
+			}
+			else
+			{
+				showUsage();
+			}
+		}
+		else if(std::strcmp(argv[i], "--texture_angle") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				textureAngle = uStr2Float(argv[i])*M_PI/180.0f;
 			}
 			else
 			{
@@ -296,6 +322,23 @@ int main(int argc, char * argv[])
 		{
 			camProjectionKeepAll = true;
 		}
+		else if(std::strcmp(argv[i], "--cam_projection_decimation") == 0)
+		{
+			++i;
+			if(i<argc-1)
+			{
+				cameraProjDecimation = uStr2Int(argv[i]);
+				if(cameraProjDecimation<1)
+				{
+					printf("--cam_projection_decimation cannot be <1! value=\"%s\"\n", argv[i]);
+					showUsage();
+				}
+			}
+			else
+			{
+				showUsage();
+			}
+		}
 		else if(std::strcmp(argv[i], "--poses") == 0)
 		{
 			exportPoses = true;
@@ -871,46 +914,58 @@ int main(int argc, char * argv[])
 		pcl::IndicesPtr indices(new std::vector<int>);
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 		pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
-		if(cloudFromScan)
+		if(node.getWeight() != -1)
 		{
-			cv::Mat tmpDepth;
+			if(cloudFromScan)
-			LaserScan 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);
+				cv::Mat tmpDepth;
-			}
+				LaserScan scan;
-			if(scan.hasRGB())
+				node.sensorData().uncompressData(exportImages?&rgb:0, (texture||exportImages)&&!node.sensorData().depthOrRightCompressed().empty()?&tmpDepth:0, &scan);
-			{
+				if(scan.empty())
-				cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
-				if(noiseRadius>0.0f && noiseMinNeighbors>0)
 				{
-					indices = util3d::radiusFiltering(cloud, noiseRadius, noiseMinNeighbors);
+					printf("Node %d doesn't have scan data, empty cloud is created.\n", iter->first);
+				}
+				if(decimation>1 || maxRange)
+				{
+					scan = util3d::commonFiltering(scan, decimation, 0, maxRange);
+				}
+				if(scan.hasRGB())
+				{
+					cloud = util3d::laserScanToPointCloudRGB(scan, scan.localTransform());
+					if(noiseRadius>0.0f && noiseMinNeighbors>0)
+					{
+						indices = util3d::radiusFiltering(cloud, noiseRadius, noiseMinNeighbors);
+					}
+				}
+				else
+				{
+					cloudI = util3d::laserScanToPointCloudI(scan, scan.localTransform());
+					if(noiseRadius>0.0f && noiseMinNeighbors>0)
+					{
+						indices = util3d::radiusFiltering(cloudI, noiseRadius, noiseMinNeighbors);
+					}
 				}
 			}
 			else
 			{
-				cloudI = util3d::laserScanToPointCloudI(scan, scan.localTransform());
+				node.sensorData().uncompressData(&rgb, &depth);
+				if(depth.empty())
+				{
+					printf("Node %d doesn't have depth or stereo data, empty cloud is "
+							"created (if you want to create point cloud from scan, use --scan option).\n", iter->first);
+				}
+				cloud = util3d::cloudRGBFromSensorData(
+						node.sensorData(),
+						decimation,      // image decimation before creating the clouds
+						maxRange,        // maximum depth of the cloud
+						0.0f,
+						indices.get());
 				if(noiseRadius>0.0f && noiseMinNeighbors>0)
 				{
-					indices = util3d::radiusFiltering(cloudI, noiseRadius, noiseMinNeighbors);
+					indices = util3d::radiusFiltering(cloud, indices, noiseRadius, noiseMinNeighbors);
 				}
 			}
 		}
-		else
-		{
-			node.sensorData().uncompressData(&rgb, &depth);
-			cloud = util3d::cloudRGBFromSensorData(
-					node.sensorData(),
-					decimation,      // image decimation before creating the clouds
-					maxRange,        // maximum depth of the cloud
-					0.0f,
-					indices.get());
-			if(noiseRadius>0.0f && noiseMinNeighbors>0)
-			{
-				indices = util3d::radiusFiltering(cloud, indices, noiseRadius, noiseMinNeighbors);
-			}
-		}
 
 		if(exportImages && !rgb.empty())
 		{
@@ -1061,6 +1116,8 @@ int main(int argc, char * argv[])
 	if(imagesExported>0)
 		printf("%d images exported!\n", imagesExported);
 
+	ConsoleProgessState progressState;
+
 	if(!mergedClouds->empty() || !mergedCloudsI->empty())
 	{
 		if(saveInDb)
@@ -1139,6 +1196,25 @@ int main(int argc, char * argv[])
 		if(camProjection && !robotPoses.empty())
 		{
 			printf("Camera projection...\n");
+			std::map<int, std::vector<rtabmap::CameraModel> > cameraModelsProj;
+			if(cameraProjDecimation>1)
+			{
+				for(std::map<int, std::vector<rtabmap::CameraModel> >::iterator iter=cameraModels.begin();
+						iter!=cameraModels.end();
+						++iter)
+				{
+					std::vector<rtabmap::CameraModel> models;
+					for(size_t i=0; i<iter->second.size(); ++i)
+					{
+						models.push_back(iter->second[i].scaled(1.0/double(cameraProjDecimation)));
+					}
+					cameraModelsProj.insert(std::make_pair(iter->first, models));
+				}
+			}
+			else
+			{
+				cameraModelsProj = cameraModels;
+			}
 			pointToCamId.resize(!cloudToExport->empty()?cloudToExport->size():cloudIToExport->size());
 			std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
 			if(!cloudToExport->empty())
@@ -1146,120 +1222,168 @@ int main(int argc, char * argv[])
 				pointToPixel = util3d::projectCloudToCameras(
 						*cloudToExport,
 						robotPoses,
-						cameraModels,
+						cameraModelsProj,
 						textureRange,
-						0,
+						textureAngle,
 						std::vector<float>(),
-						distanceToCamPolicy);
+						distanceToCamPolicy,
+						&progressState);
 			}
 			else if(!cloudIToExport->empty())
 			{
 				pointToPixel = util3d::projectCloudToCameras(
 						*cloudIToExport,
 						robotPoses,
-						cameraModels,
+						cameraModelsProj,
 						textureRange,
-						0,
+						textureAngle,
 						std::vector<float>(),
-						distanceToCamPolicy);
+						distanceToCamPolicy,
+						&progressState);
 				pointToCamIntensity.resize(pointToPixel.size());
 			}
 
+			printf("Camera projection... coloring the cloud\n");
 			// 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;
+			int imagesDone = 1;
+			for(std::map<int, rtabmap::Transform>::iterator iter=robotPoses.begin(); iter!=robotPoses.end(); ++iter)
+			{
+				int nodeID = iter->first;
+				cv::Mat image;
+				if(uContains(nodes,nodeID) && !nodes.at(nodeID).sensorData().imageCompressed().empty())
+				{
+					nodes.at(nodeID).sensorData().uncompressDataConst(&image, 0);
+				}
+				if(!image.empty())
+				{
+					if(cameraProjDecimation>1)
+					{
+						image = util2d::decimate(image, cameraProjDecimation);
+					}
+					UASSERT(cameraModelsProj.find(nodeID) != cameraModelsProj.end());
+					int modelsSize = cameraModelsProj.at(nodeID).size();
+					for(size_t i=0; i<pointToPixel.size(); ++i)
+					{
+						int cameraIndex = pointToPixel[i].first.second;
+						if(nodeID == pointToPixel[i].first.first && cameraIndex>=0)
+						{
+							pcl::PointXYZRGBNormal pt;
+							float intensity = 0;
+							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;
+								intensity = cloudIToExport->at(i).intensity;
+							}
+
+							int subImageWidth = image.cols / modelsSize;
+							cv::Mat subImage = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth));
+
+							int x = pointToPixel[i].second.x * (float)subImage.cols;
+							int y = pointToPixel[i].second.y * (float)subImage.rows;
+							UASSERT(x>=0 && x<subImage.cols);
+							UASSERT(y>=0 && y<subImage.rows);
+
+							if(subImage.type()==CV_8UC3)
+							{
+								cv::Vec3b bgr = subImage.at<cv::Vec3b>(y, x);
+								pt.b = bgr[0];
+								pt.g = bgr[1];
+								pt.r = bgr[2];
+							}
+							else
+							{
+								UASSERT(subImage.type()==CV_8UC1);
+								pt.r = pt.g = pt.b = subImage.at<unsigned char>(pointToPixel[i].second.y * subImage.rows, pointToPixel[i].second.x * subImage.cols);
+							}
+
+							int exportedId = nodeID;
+							pointToCamId[i] = exportedId;
+							if(!pointToCamIntensity.empty())
+							{
+								pointToCamIntensity[i] = intensity;
+							}
+							assembledCloudValidPoints->at(i) = pt;
+						}
+					}
+				}
+				UINFO("Processed %d/%d images", imagesDone++, (int)robotPoses.size());
+			}
+
+			pcl::IndicesPtr validIndices(new std::vector<int>(pointToPixel.size()));
+			size_t oi = 0;
 			for(size_t i=0; i<pointToPixel.size(); ++i)
 			{
-				pcl::PointXYZRGBNormal pt;
+				if(pointToPixel[i].first.first <=0)
-				float intensity = 0;
-				if(!cloudToExport->empty())
 				{
-					pt = cloudToExport->at(i);
+					if(camProjectionKeepAll)
-				}
-				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;
-					intensity = cloudIToExport->at(i).intensity;
-				}
-				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);
+						pcl::PointXYZRGBNormal pt;
-					}
+						float intensity = 0;
-					else if(uContains(nodes,nodeID) && !nodes.at(nodeID).sensorData().imageCompressed().empty())
+						if(!cloudToExport->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 = cloudToExport->at(i);
-							pt.b = bgr[0];
-							pt.g = bgr[1];
-							pt.r = bgr[2];
 						}
-						else
+						else if(!cloudIToExport->empty())
 						{
-							UASSERT(image.type()==CV_8UC1);
+							pt.x = cloudIToExport->at(i).x;
-							pt.r = pt.g = pt.b = image.at<unsigned char>(pointToPixel[i].second.y * image.rows, pointToPixel[i].second.x * image.cols);
+							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;
+							intensity = cloudIToExport->at(i).intensity;
 						}
-					}
 
-					int exportedId = nodeID;
+						pointToCamId[i] = 0; // invalid
-					pointToCamId[oi] = exportedId;
+						pt.b = 0;
-					if(!pointToCamIntensity.empty())
+						pt.g = 0;
-					{
+						pt.r = 255;
-						pointToCamIntensity[oi] = intensity;
+						if(!pointToCamIntensity.empty())
+						{
+							pointToCamIntensity[i] = intensity;
+						}
+						assembledCloudValidPoints->at(i) = pt; // red
+						validIndices->at(oi++) = i;
 					}
-					assembledCloudValidPoints->at(oi++) = pt;
 				}
-				else if(camProjectionKeepAll)
+				else
 				{
-					pointToCamId[oi] = 0; // invalid
+					validIndices->at(oi++) = i;
-					pt.b = 0;
-					pt.g = 0;
-					pt.r = 255;
-					if(!pointToCamIntensity.empty())
-					{
-						pointToCamIntensity[oi] = intensity;
-					}
-					assembledCloudValidPoints->at(oi++) = pt; // red
 				}
 			}
 
-			assembledCloudValidPoints->resize(oi);
+			if(oi != validIndices->size())
+			{
+				validIndices->resize(oi);
+				assembledCloudValidPoints = util3d::extractIndices(assembledCloudValidPoints, validIndices, false, false);
+				std::vector<int> pointToCamIdTmp(validIndices->size());
+				std::vector<float> pointToCamIntensityTmp(validIndices->size());
+				for(size_t i=0; i<validIndices->size(); ++i)
+				{
+					pointToCamIdTmp[i] = pointToCamId[validIndices->at(i)];
+					pointToCamIntensityTmp[i] = pointToCamIntensity[validIndices->at(i)];
+				}
+				pointToCamId = pointToCamIdTmp;
+				pointToCamIntensity = pointToCamIntensityTmp;
+				pointToCamIdTmp.clear();
+				pointToCamIntensityTmp.clear();
+			}
+
 			cloudToExport = assembledCloudValidPoints;
 			cloudIToExport->clear();
-			pointToCamId.resize(oi);
-			if(!pointToCamIntensity.empty())
-			{
-				pointToCamIntensity.resize(oi);
-			}
 
 			printf("Camera projection... done! (%fs)\n", timer.ticks());
 		}
@@ -1408,10 +1532,10 @@ int main(int argc, char * argv[])
 							cameraDepths,
 							textureRange,
 							textureDepthError,
-							0.0f,
+							textureAngle,
 							multiband?0:50, // Min polygons in camera view to be textured by this camera
 							std::vector<float>(),
-							0,
+							&progressState,
 							&vertexToPixels,
 							distanceToCamPolicy);
 					printf("Texturing... done (%fs).\n", timer.ticks());