From 3cf8ea63d8c28b0cfe1ad827ebe4ddb06a1f3046 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Fri, 3 Mar 2017 15:22:20 -0500
Subject: [PATCH] Fixed a bug where camera facing polygons were not textured

---
 .../pcl18/surface/impl/texture_mapping.hpp    | 21 ++++++++++---------
 guilib/include/rtabmap/gui/ProgressDialog.h   |  2 +-
 guilib/src/ExportCloudsDialog.cpp             |  4 ++--
 guilib/src/ProgressDialog.cpp                 |  8 +++----
 guilib/src/TexturingState.h                   |  1 +
 5 files changed, 19 insertions(+), 17 deletions(-)

diff --git a/corelib/src/pcl18/surface/impl/texture_mapping.hpp b/corelib/src/pcl18/surface/impl/texture_mapping.hpp
index fc43e532..4a75e832 100644
--- a/corelib/src/pcl18/surface/impl/texture_mapping.hpp
+++ b/corelib/src/pcl18/surface/impl/texture_mapping.hpp
@@ -1103,17 +1103,16 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 			{
 				// check if the polygon is facing the camera, assuming counterclockwise normal
 				Eigen::Vector3f v0(
-						pt1.x - pt0.x,
-						pt1.y - pt0.y,
-						pt1.z - pt0.z);
+						uv_coords2.x - uv_coords1.x,
+						uv_coords2.y - uv_coords1.y,
+						0);
 				Eigen::Vector3f v1(
-						pt2.x - pt0.x,
-						pt2.y - pt0.y,
-						pt2.z - pt0.z);
+						uv_coords3.x - uv_coords1.x,
+						uv_coords3.y - uv_coords1.y,
+						0);
 				Eigen::Vector3f normal = v0.cross(v1);
-				float angle = normal.dot(Eigen::Vector3f(0.0f,0.0f,-1.0f));
+				float angle = normal.dot(Eigen::Vector3f(0.0f,0.0f,1.0f));
 				bool facingTheCam = angle>0.0f;
-
 				float distanceToCam = std::min(std::min(pt0.z, pt1.z), pt2.z);
 				pcl::PointXY center;
 				center.x = (uv_coords1.x+uv_coords2.x+uv_coords3.x)/3.0f;
@@ -1254,12 +1253,13 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 		UWARN("Texturing cancelled!");
 		return false;
 	}
+	int textured = 0;
 	for(unsigned int idx_face=0; idx_face<faces.size(); ++idx_face)
 	{
 		if((idx_face+1)%10000 == 0)
 		{
 			UDEBUG("face %d/%d", idx_face+1, (int)faces.size());
-			if(state && !state->callback(uFormat("Textured %d/%d polygons", (int)idx_face+1, (int)faces.size())))
+			if(state && !state->callback(uFormat("Textured %d/%d of %d polygons", textured, idx_face+1, (int)faces.size())))
 			{
 				//cancelled!
 				UWARN("Texturing cancelled!");
@@ -1293,6 +1293,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 
 		if(cameraIndex >= 0)
 		{
+			++textured;
 			mesh.tex_polygons[cameraIndex].push_back(face);
 			mesh.tex_coordinates[cameraIndex].push_back(Eigen::Vector2f(uv_coords[0].x, uv_coords[0].y));
 			mesh.tex_coordinates[cameraIndex].push_back(Eigen::Vector2f(uv_coords[1].x, uv_coords[1].y));
@@ -1306,7 +1307,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
 			mesh.tex_coordinates[cameras.size()].push_back(Eigen::Vector2f(-1.0,-1.0));
 		}
 	}
-	UINFO("Process %d polygons...done!", (int)faces.size());
+	UINFO("Process %d polygons...done! (%d textured)", (int)faces.size(), textured);
 	return true;
 }
 
diff --git a/guilib/include/rtabmap/gui/ProgressDialog.h b/guilib/include/rtabmap/gui/ProgressDialog.h
index b4d5ac3a..f9ced769 100644
--- a/guilib/include/rtabmap/gui/ProgressDialog.h
+++ b/guilib/include/rtabmap/gui/ProgressDialog.h
@@ -63,7 +63,7 @@ protected:
 
 public slots:
 	void appendText(const QString & text ,const QColor & color = Qt::black);
-	void incrementStep();
+	void incrementStep(int steps = 1);
 	void clear();
 	void resetProgress();
 
diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp
index 0cd58808..88bfa49f 100644
--- a/guilib/src/ExportCloudsDialog.cpp
+++ b/guilib/src/ExportCloudsDialog.cpp
@@ -889,7 +889,6 @@ bool ExportCloudsDialog::getExportedClouds(
 		{
 			mul+=1;
 		}
-		mul+=1; // normals
 		if(_ui->checkBox_textureMapping->isChecked())
 		{
 			mul+=1;
@@ -1720,7 +1719,7 @@ bool ExportCloudsDialog::getExportedClouds(
 
 					meshes.insert(std::make_pair(iter->first, mesh));
 
-					_progressDialog->incrementStep();
+					_progressDialog->incrementStep(_ui->checkBox_assemble->isChecked()?poses.size():1);
 					QApplication::processEvents();
 					if(_canceled)
 					{
@@ -1877,6 +1876,7 @@ bool ExportCloudsDialog::getExportedClouds(
 						}
 
 						TexturingState texturingState(_progressDialog);
+						_progressDialog->setMaximumSteps(_progressDialog->maximumSteps()+iter->second->polygons.size()/10000+1);
 						textureMesh = util3d::createTextureMesh(
 								iter->second,
 								cameraPoses,
diff --git a/guilib/src/ProgressDialog.cpp b/guilib/src/ProgressDialog.cpp
index 895df60c..45d762f0 100644
--- a/guilib/src/ProgressDialog.cpp
+++ b/guilib/src/ProgressDialog.cpp
@@ -133,14 +133,14 @@ void ProgressDialog::setMaximumSteps(int steps)
 	_progressBar->setMaximum(steps);
 }
 
-void ProgressDialog::incrementStep()
+void ProgressDialog::incrementStep(int steps)
 {
 	//incremental progress bar (if we don't know how many items will be added)
-	if(_progressBar->value() == _progressBar->maximum()-1)
+	if(_progressBar->value() >= _progressBar->maximum()-steps)
 	{
-		_progressBar->setMaximum(_progressBar->maximum()+1);
+		_progressBar->setMaximum(_progressBar->maximum()+steps+1);
 	}
-	_progressBar->setValue(_progressBar->value()+1);
+	_progressBar->setValue(_progressBar->value()+steps);
 }
 
 void ProgressDialog::clear()
diff --git a/guilib/src/TexturingState.h b/guilib/src/TexturingState.h
index ab58d992..2adf875b 100644
--- a/guilib/src/TexturingState.h
+++ b/guilib/src/TexturingState.h
@@ -50,6 +50,7 @@ public:
 		if(!msg.empty())
 		{
 			dialog_->appendText(msg.c_str());
+			dialog_->incrementStep();
 		}
 		QApplication::processEvents();
 		return !canceled_;