From 4db957f43775f250387d551fa4bf8c8841a71088 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sat, 4 Mar 2017 02:53:11 -0500
Subject: [PATCH] Increased polygon texturing step speed

---
 corelib/src/Rtabmap.cpp                            | 1 +
 corelib/src/pcl18/surface/impl/texture_mapping.hpp | 8 ++++++--
 guilib/src/ExportCloudsDialog.cpp                  | 6 +++---
 guilib/src/MainWindow.cpp                          | 5 +++++
 4 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp
index 6f62eb88..5b9e7667 100644
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -1396,6 +1396,7 @@ bool Rtabmap::process(
 					// When analyzing logs, it's convenient to know
 					// if the hypothesis would be rejected if T_loop would be lower.
 					rejectedHypothesis = true;
+					UDEBUG("rejected hypothesis: under loop ratio %f < %f", _highestHypothesis.second, _loopRatio*lastHighestHypothesis.second);
 				}
 
 				//for statistic...
diff --git a/corelib/src/pcl18/surface/impl/texture_mapping.hpp b/corelib/src/pcl18/surface/impl/texture_mapping.hpp
index 4a75e832..f52e0d1a 100644
--- a/corelib/src/pcl18/surface/impl/texture_mapping.hpp
+++ b/corelib/src/pcl18/surface/impl/texture_mapping.hpp
@@ -1068,6 +1068,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 	// pre compute all cam inverse and visibility
 	std::vector<std::map<int, FaceInfo > > visibleFaces(cameras.size());
 	std::vector<Eigen::Affine3f> invCamTransform(cameras.size());
+	std::vector<std::list<int> > faceCameras(faces.size());
 	UINFO("Precompute visible faces per cam (%d faces, %d cams)", (int)faces.size(), (int)cameras.size());
 	for (unsigned int current_cam = 0; current_cam < cameras.size(); ++current_cam)
 	{
@@ -1219,6 +1220,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 			for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
 			{
 				polygonsKept.insert(polygon_to_face_index[*jter]);
+				faceCameras[polygon_to_face_index[*jter]].push_back(current_cam);
 			}
 		}
 
@@ -1271,10 +1273,12 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 		int cameraIndex = -1;
 		float closestDistanceToCam = std::numeric_limits<float>::max();
 		pcl::PointXY uv_coords[3];
-		for (unsigned int current_cam = 0; current_cam < cameras.size(); ++current_cam)
+		for (std::list<int>::iterator camIter = faceCameras[idx_face].begin(); camIter!=faceCameras[idx_face].end(); ++camIter)
 		{
+			int current_cam = *camIter;
 			std::map<int, FaceInfo>::iterator iter = visibleFaces[current_cam].find(idx_face);
-			if (iter != visibleFaces[current_cam].end() && iter->second.facingTheCam)
+			UASSERT(iter != visibleFaces[current_cam].end());
+			if (iter->second.facingTheCam)
 			{
 				float distanceToCam = iter->second.distance;
 
diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp
index 88bfa49f..bebeec86 100644
--- a/guilib/src/ExportCloudsDialog.cpp
+++ b/guilib/src/ExportCloudsDialog.cpp
@@ -685,9 +685,6 @@ void ExportCloudsDialog::viewClouds(
 		{
 			for (std::map<int, pcl::TextureMesh::Ptr>::iterator iter = textureMeshes.begin(); iter != textureMeshes.end(); ++iter)
 			{
-				_progressDialog->appendText(tr("Viewing the mesh %1 (%2 polygons)...").arg(iter->first).arg(iter->second->tex_polygons.size() ? iter->second->tex_polygons[0].size() : 0));
-				_progressDialog->incrementStep();
-
 				pcl::TextureMesh::Ptr mesh = iter->second;
 
 				// As CloudViewer is not supporting more than one texture per mesh, merge them all by default
@@ -697,6 +694,9 @@ void ExportCloudsDialog::viewClouds(
 					globalTexture = mergeTextures(*mesh, cachedSignatures);
 				}
 
+				_progressDialog->appendText(tr("Viewing the mesh %1 (%2 polygons)...").arg(iter->first).arg(mesh->tex_polygons.size()?mesh->tex_polygons[0].size():0));
+				_progressDialog->incrementStep();
+
 				// VTK issue:
 				//  tex_coordinates should be linked to points, not
 				//  polygon vertices. Points linked to multiple different TCoords (different textures) should
diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp
index c41e5d19..5fa0c6c9 100644
--- a/guilib/src/MainWindow.cpp
+++ b/guilib/src/MainWindow.cpp
@@ -4789,6 +4789,11 @@ void MainWindow::postProcessing()
 							{
 								Transform transform;
 								RegistrationInfo info;
+								if(parameters.find(Parameters::kRegStrategy()) != parameters.end() &&
+									parameters.at(Parameters::kRegStrategy()).compare("1") == 0)
+								{
+									uInsert(parameters, ParametersPair(Parameters::kRegStrategy(), "2"));
+								}
 								Registration * registration = Registration::create(parameters);
 								transform = registration->computeTransformation(signatureFrom, signatureTo, Transform(), &info);
 								delete registration;