Tango: replaced RTABMapApp::mergeTextures() by util3d::mergeTextures()

app/android/jni/ProgressionStatus.h

@@ -70,7 +70,7 @@ public:
 			increment();
 		}
 
-		return !isCanceled();
+		return ProgressState::callback(msg);
 	}
 	virtual ~ProgressionStatus(){}
 

app/android/jni/RTABMapApp.cpp

@@ -1880,425 +1880,6 @@ void RTABMapApp::save(const std::string & databasePath)
 
 }
 
-double sqr(uchar v)
-{
-	return double(v)*double(v);
-}
-
-std::vector<cv::Mat> RTABMapApp::mergeTextures(
-		pcl::TextureMesh & mesh,
-		int textureSize,
-		int textureCount,
-		const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels) const
-{
-	UASSERT(textureSize> 0);
-	LOGD("textureSize = %d textureCount=%d materials=%d", textureSize, textureCount, mesh.tex_materials.size());
-	std::vector<cv::Mat> globalTextures;
-	if(mesh.tex_materials.size() >= 1)
-	{
-		std::vector<int> textures(mesh.tex_materials.size(), -1);
-		cv::Size imageSize;
-		int imageType=CV_8UC3;
-		for(unsigned int i=0; i<mesh.tex_materials.size(); ++i)
-		{
-			if(!mesh.tex_materials[i].tex_file.empty() &&
-				mesh.tex_polygons[i].size() &&
-			   uIsInteger(mesh.tex_materials[i].tex_file, false))
-			{
-				int textureId = uStr2Int(mesh.tex_materials[i].tex_file);
-				textures[i] = textureId;
-
-				if(imageSize.height == 0)
-				{
-					rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(textureId);
-
-					UASSERT(!data.imageCompressed().empty() &&
-						data.cameraModels().size()==1 &&
-						data.cameraModels()[0].imageHeight()>0);
-					imageSize = data.cameraModels()[0].imageSize();
-				}
-			}
-			else
-			{
-				textures[i] = -1;
-			}
-		}
-		if(textures.size() && imageSize.height>0 && imageSize.width>0)
-		{
-			float scale = 0.0f;
-			std::vector<bool> materialsKept;
-			rtabmap::util3d::concatenateTextureMaterials(mesh, imageSize, textureSize, textureCount, scale, &materialsKept);
-			LOGD("scale=%f materials=%d", scale, (int)mesh.tex_materials.size());
-			if(scale && mesh.tex_materials.size())
-			{
-				int materials = (int)mesh.tex_materials.size();
-				int cols = float(textureSize)/(scale*imageSize.width);
-				int rows = float(textureSize)/(scale*imageSize.height);
-
-				globalTextures.resize(materials);
-				for(int i=0; i<materials; ++i)
-				{
-					globalTextures[i] = cv::Mat(textureSize, textureSize, imageType, cv::Scalar::all(255));
-				}
-
-				// make a blank texture
-				cv::Mat emptyImage(int(imageSize.height*scale), int(imageSize.width*scale), imageType, cv::Scalar::all(255));
-				cv::Mat emptyImageMask(int(imageSize.height*scale), int(imageSize.width*scale), CV_8UC1, cv::Scalar::all(255));
-				int oi=0;
-				std::vector<cv::Point2i> imageOrigin(textures.size());
-				std::vector<int> newCamIndex(textures.size(), -1);
-				for(int i=0; i<(int)textures.size(); ++i)
-				{
-					if(materialsKept.at(i))
-					{
-						int indexMaterial = oi / (cols*rows);
-						UASSERT(indexMaterial < materials);
-
-						int u = oi%cols * emptyImage.cols;
-						int v = ((oi/cols) % rows ) * emptyImage.rows;
-						UASSERT(u < textureSize-emptyImage.cols);
-						UASSERT(v < textureSize-emptyImage.rows);
-						newCamIndex[i] = oi;
-						imageOrigin[i].x = u;
-						imageOrigin[i].y = v;
-						if(textures[i]>=0)
-						{
-							rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(textures[i]);
-							UASSERT_MSG(!data.imageCompressed().empty(), uFormat("id=%d", textures[i]).c_str());
-							cv::Mat image;
-							data.uncompressDataConst(&image, 0);
-							UASSERT(!image.empty());
-							cv::Mat resizedImage;
-							cv::resize(image, resizedImage, emptyImage.size(), 0.0f, 0.0f, cv::INTER_AREA);
-							if(vertexToPixels.empty() &&
-								createdMeshes_.find(textures[i]) != createdMeshes_.end() &&
-								(createdMeshes_.at(textures[i]).gains[0] != 1.0 || createdMeshes_.at(textures[i]).gains[1] != 1.0 || createdMeshes_.at(textures[i]).gains[2] != 1.0))
-							{
-								std::vector<cv::Mat> channels;
-								cv::split(resizedImage, channels);
-								// assuming BGR
-								cv::multiply(channels[0], createdMeshes_.at(textures[i]).gains[2], channels[0]);
-								cv::multiply(channels[1], createdMeshes_.at(textures[i]).gains[1], channels[1]);
-								cv::multiply(channels[2], createdMeshes_.at(textures[i]).gains[0], channels[2]);
-								cv::merge(channels, resizedImage);
-							}
-							if(resizedImage.type() == CV_8UC1)
-							{
-								cv::Mat resizedImageColor;
-								cv::cvtColor(resizedImage,resizedImageColor,CV_GRAY2RGB);
-								resizedImage = resizedImageColor;
-							}
-							UASSERT(resizedImage.type() == globalTextures[indexMaterial].type());
-							resizedImage.copyTo(globalTextures[indexMaterial](cv::Rect(u, v, resizedImage.cols, resizedImage.rows)));
-						}
-						else
-						{
-							emptyImage.copyTo(globalTextures[indexMaterial](cv::Rect(u, v, emptyImage.cols, emptyImage.rows)));
-						}
-						++oi;
-					}
-
-					if(progressionStatus_.isCanceled())
-					{
-						return cv::Mat();
-					}
-
-					progressionStatus_.increment();
-				}
-
-				if(vertexToPixels.size())
-				{
-					LOGD("gain compensation");
-					// gain compensation
-					const int num_images = static_cast<int>(oi);
-					cv::Mat_<int> N(num_images, num_images); N.setTo(0);
-					cv::Mat_<double> I(num_images, num_images); I.setTo(0);
-
-					cv::Mat_<double> IR(num_images, num_images); IR.setTo(0);
-					cv::Mat_<double> IG(num_images, num_images); IG.setTo(0);
-					cv::Mat_<double> IB(num_images, num_images); IB.setTo(0);
-
-					// Adjust UV coordinates to globalTexture
-					for(unsigned int p=0; p<vertexToPixels.size(); ++p)
-					{
-						for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[p].begin(); iter!=vertexToPixels[p].end(); ++iter)
-						{
-							if(materialsKept.at(iter->first))
-							{
-								N(newCamIndex[iter->first], newCamIndex[iter->first]) +=1;
-
-								std::map<int, pcl::PointXY>::const_iterator jter=iter;
-								++jter;
-								int k = 1;
-								for(; jter!=vertexToPixels[p].end(); ++jter, ++k)
-								{
-									if(materialsKept.at(jter->first))
-									{
-										int i = newCamIndex[iter->first];
-										int j = newCamIndex[jter->first];
-
-										N(i, j) += 1;
-										N(j, i) += 1;
-
-										int indexMaterial = i / (cols*rows);
-
-										// uv in globalTexture
-										int ui = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
-										int vi = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
-										int uj = jter->second.x*emptyImage.cols + imageOrigin[jter->first].x;
-										int vj = (1.0-jter->second.y)*emptyImage.rows + imageOrigin[jter->first].y;
-										cv::Vec3b * pt1 = globalTextures[indexMaterial].ptr<cv::Vec3b>(vi,ui);
-										cv::Vec3b * pt2 = globalTextures[indexMaterial].ptr<cv::Vec3b>(vj,uj);
-
-										I(i, j) += std::sqrt(static_cast<double>(sqr(pt1->val[0]) + sqr(pt1->val[1]) + sqr(pt1->val[2])));
-										I(j, i) += std::sqrt(static_cast<double>(sqr(pt2->val[0]) + sqr(pt2->val[1]) + sqr(pt2->val[2])));
-
-										IR(i, j) += static_cast<double>(pt1->val[2]);
-										IR(j, i) += static_cast<double>(pt2->val[2]);
-										IG(i, j) += static_cast<double>(pt1->val[1]);
-										IG(j, i) += static_cast<double>(pt2->val[1]);
-										IB(i, j) += static_cast<double>(pt1->val[0]);
-										IB(j, i) += static_cast<double>(pt2->val[0]);
-									}
-								}
-							}
-						}
-					}
-
-					for(int i=0; i<num_images; ++i)
-					{
-						for(int j=i; j<num_images; ++j)
-						{
-							if(i == j)
-							{
-								if(N(i,j) == 0)
-								{
-									N(i,j) = 1;
-								}
-							}
-							else if(N(i, j))
-							{
-								I(i, j) /= N(i, j);
-								I(j, i) /= N(j, i);
-
-								IR(i, j) /= N(i, j);
-								IR(j, i) /= N(j, i);
-								IG(i, j) /= N(i, j);
-								IG(j, i) /= N(j, i);
-								IB(i, j) /= N(i, j);
-								IB(j, i) /= N(j, i);
-							}
-						}
-					}
-
-					cv::Mat_<double> A(num_images, num_images); A.setTo(0);
-					cv::Mat_<double> b(num_images, 1); b.setTo(0);
-					cv::Mat_<double> AR(num_images, num_images); AR.setTo(0);
-					cv::Mat_<double> AG(num_images, num_images); AG.setTo(0);
-					cv::Mat_<double> AB(num_images, num_images); AB.setTo(0);
-					double alpha = 0.01;
-					double beta = 10.0;
-					for (int i = 0; i < num_images; ++i)
-					{
-						for (int j = 0; j < num_images; ++j)
-						{
-							b(i, 0) += beta * N(i, j);
-							A(i, i) += beta * N(i, j);
-							AR(i, i) += beta * N(i, j);
-							AG(i, i) += beta * N(i, j);
-							AB(i, i) += beta * N(i, j);
-							if (j == i) continue;
-							A(i, i) += 2 * alpha * I(i, j) * I(i, j) * N(i, j);
-							A(i, j) -= 2 * alpha * I(i, j) * I(j, i) * N(i, j);
-
-							AR(i, i) += 2 * alpha * IR(i, j) * IR(i, j) * N(i, j);
-							AR(i, j) -= 2 * alpha * IR(i, j) * IR(j, i) * N(i, j);
-
-							AG(i, i) += 2 * alpha * IG(i, j) * IG(i, j) * N(i, j);
-							AG(i, j) -= 2 * alpha * IG(i, j) * IG(j, i) * N(i, j);
-
-							AB(i, i) += 2 * alpha * IB(i, j) * IB(i, j) * N(i, j);
-							AB(i, j) -= 2 * alpha * IB(i, j) * IB(j, i) * N(i, j);
-						}
-					}
-
-					cv::Mat_<double> gainsGray, gainsR, gainsG, gainsB;
-					cv::solve(A, b, gainsGray);
-
-					cv::solve(AR, b, gainsR);
-					cv::solve(AG, b, gainsG);
-					cv::solve(AB, b, gainsB);
-
-					cv::Mat_<double> gains(gainsGray.rows, 4);
-					gainsGray.copyTo(gains.col(0));
-					gainsR.copyTo(gains.col(1));
-					gainsG.copyTo(gains.col(2));
-					gainsB.copyTo(gains.col(3));
-
-					for(int t=0; t<(int)textures.size(); ++t)
-					{
-						//break;
-						if(materialsKept.at(t))
-						{
-							int u = imageOrigin[t].x;
-							int v = imageOrigin[t].y;
-
-							int indexMaterial = newCamIndex[t] / (cols*rows);
-							cv::Mat roi = globalTextures[indexMaterial](cv::Rect(u, v, emptyImage.cols, emptyImage.rows));
-
-							std::vector<cv::Mat> channels;
-							cv::split(roi, channels);
-							// assuming BGR
-							cv::multiply(channels[0], gains(newCamIndex[t], 3), channels[0]);
-							cv::multiply(channels[1], gains(newCamIndex[t], 2), channels[1]);
-							cv::multiply(channels[2], gains(newCamIndex[t], 1), channels[2]);
-							cv::merge(channels, roi);
-						}
-					}
-					progressionStatus_.increment();
-
-					// blending BGR
-					LOGD("blending");
-					int decimation = 0;
-
-					// determinate decimation to apply
-					std::vector<float> edgeLengths;
-					if(mesh.tex_coordinates.size() && mesh.tex_coordinates[0].size())
-					{
-						UASSERT(mesh.tex_polygons.size() && mesh.tex_polygons[0].size() && mesh.tex_polygons[0][0].vertices.size());
-						int polygonSize = mesh.tex_polygons[0][0].vertices.size();
-
-						for(unsigned int i=0; i<mesh.tex_coordinates[0].size(); i+=polygonSize)
-						{
-							for(int j=0; j<polygonSize; ++j)
-							{
-								const Eigen::Vector2f & uc1 = mesh.tex_coordinates[0][i + j];
-								const Eigen::Vector2f & uc2 = mesh.tex_coordinates[0][i + (j+1)%polygonSize];
-								Eigen::Vector2f edge = (uc1-uc2)*textureSize;
-								edgeLengths.push_back(fabs(edge[0]));
-								edgeLengths.push_back(fabs(edge[1]));
-							}
-						}
-						float edgeLength = 0.0f;
-						if(edgeLengths.size())
-						{
-							std::sort(edgeLengths.begin(), edgeLengths.end());
-							float m = uMean(edgeLengths.data(), edgeLengths.size());
-							float stddev = std::sqrt(uVariance(edgeLengths.data(), edgeLengths.size(), m));
-							edgeLength = m+stddev;
-							decimation = 1 << 6;
-							for(int i=1; i<=6; ++i)
-							{
-								if(float(1 << i) >= edgeLength)
-								{
-									decimation = 1 << i;
-									break;
-								}
-							}
-						}
-					}
-
-					if(decimation>0)
-					{
-						LOGD("blending decimation=%d", decimation);
-						std::vector<cv::Mat> blendGains(materials);
-						for(int i=0; i<materials;++i)
-						{
-							blendGains[i] = cv::Mat(globalTextures[i].rows/decimation, globalTextures[i].cols/decimation, CV_32FC3, cv::Scalar::all(1.0f));
-						}
-
-						for(unsigned int p=0; p<vertexToPixels.size(); ++p)
-						{
-							if(vertexToPixels[p].size() > 1)
-							{
-								std::vector<float> gainsB(vertexToPixels[p].size());
-								std::vector<float> gainsG(vertexToPixels[p].size());
-								std::vector<float> gainsR(vertexToPixels[p].size());
-								float sumWeight = 0.0f;
-								int k=0;
-								for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[p].begin(); iter!=vertexToPixels[p].end(); ++iter)
-								{
-									if(materialsKept.at(iter->first))
-									{
-										int u = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
-										int v = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
-										float x = iter->second.x - 0.5f;
-										float y = iter->second.y - 0.5f;
-										float weight = 0.7f - sqrt(x*x+y*y);
-										if(weight<0.0f)
-										{
-											weight = 0.0f;
-										}
-										int indexMaterial = newCamIndex[iter->first] / (cols*rows);
-										cv::Vec3b * pt = globalTextures[indexMaterial].ptr<cv::Vec3b>(v,u);
-										gainsB[k] = static_cast<double>(pt->val[0]) * weight;
-										gainsG[k] = static_cast<double>(pt->val[1]) * weight;
-										gainsR[k] = static_cast<double>(pt->val[2]) * weight;
-										sumWeight += weight;
-										++k;
-									}
-								}
-								gainsB.resize(k);
-								gainsG.resize(k);
-								gainsR.resize(k);
-
-								if(sumWeight > 0)
-								{
-									float targetColor[3];
-									targetColor[0] = uSum(gainsB.data(), gainsB.size()) / sumWeight;
-									targetColor[1] = uSum(gainsG.data(), gainsG.size()) / sumWeight;
-									targetColor[2] = uSum(gainsR.data(), gainsR.size()) / sumWeight;
-									for(std::map<int, pcl::PointXY>::const_iterator iter=vertexToPixels[p].begin(); iter!=vertexToPixels[p].end(); ++iter)
-									{
-										if(materialsKept.at(iter->first))
-										{
-											int u = iter->second.x*emptyImage.cols + imageOrigin[iter->first].x;
-											int v = (1.0-iter->second.y)*emptyImage.rows + imageOrigin[iter->first].y;
-											int indexMaterial = newCamIndex[iter->first] / (cols*rows);
-											cv::Vec3b * pt = globalTextures[indexMaterial].ptr<cv::Vec3b>(v,u);
-											float gB = targetColor[0]/(pt->val[0]==0?1.0f:pt->val[0]);
-											float gG = targetColor[1]/(pt->val[1]==0?1.0f:pt->val[1]);
-											float gR = targetColor[2]/(pt->val[2]==0?1.0f:pt->val[2]);
-											cv::Vec3f * ptr = blendGains[indexMaterial].ptr<cv::Vec3f>(v/decimation, u/decimation);
-											ptr->val[0] = (gB>1.3f)?1.3f:(gB<0.7f)?0.7f:gB;
-											ptr->val[1] = (gG>1.3f)?1.3f:(gG<0.7f)?0.7f:gG;
-											ptr->val[2] = (gR>1.3f)?1.3f:(gR<0.7f)?0.7f:gR;
-										}
-									}
-								}
-							}
-						}
-
-						LOGD("blending multiply");
-						for(int i=0; i<materials; ++i)
-						{
-							cv::Mat dst;
-							cv::blur(blendGains[i], dst, cv::Size(3,3));
-							cv::resize(dst, blendGains[i], globalTextures[i].size(), 0, 0, cv::INTER_LINEAR);
-
-							cv::multiply(globalTextures[i], blendGains[i], globalTextures[i], 1.0, CV_8UC3);
-						}
-					}
-					progressionStatus_.increment();
-				}
-			}
-			else
-			{
-				UERROR("Failed merging textures");
-			}
-		}
-		else if(textures.size() == 0)
-		{
-			UERROR("No textures kept!");
-		}
-		else
-		{
-			UERROR("No image size set!");
-		}
-	}
-	return globalTextures;
-}
-
 void RTABMapApp::cancelProcessing()
 {
 	UWARN("Processing canceled!");
@@ -3091,7 +2672,17 @@ bool RTABMapApp::exportMesh(
 				if(textureSize>0 && totalPolygons && textureMesh->tex_materials.size())
 				{
 					LOGI("Merging %d textures...", (int)textureMesh->tex_materials.size());
-					globalTextures = mergeTextures(*textureMesh, textureSize, textureCount, vertexToPixels);
+					globalTextures = rtabmap::util3d::mergeTextures(
+							*textureMesh,
+							std::map<int, cv::Mat>(),
+							std::map<int, std::vector<rtabmap::CameraModel> >(),
+							rtabmap_->getMemory(),
+							0,
+							textureSize,
+							textureCount,
+							vertexToPixels,
+							true, 10.0f, true ,true, 0, 0, 0, false,
+							&progressionStatus_);
 
 					if(progressionStatus_.isCanceled())
 					{

app/android/jni/RTABMapApp.h

@@ -149,10 +149,6 @@ class RTABMapApp : public UEventsHandler {
 
   void resetMapping();
   void save(const std::string & databasePath);
-  std::vector<cv::Mat> mergeTextures(pcl::TextureMesh & mesh,
-		  int textureSize,
-		  int textureCount,
-		  const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels) const;
   void cancelProcessing();
   bool exportMesh(
 		  const std::string & filePath,

corelib/include/rtabmap/core/util3d_surface.h

@@ -188,7 +188,8 @@ std::vector<cv::Mat> RTABMAP_EXP mergeTextures(
 		int blendingDecimation = 0,          //0=auto depending on projected polygon size and texture size
 		int brightnessContrastRatioLow = 0,  //0=disabled, values between 0 and 100
 		int brightnessContrastRatioHigh = 0, //0=disabled, values between 0 and 100
-		bool exposureFusion = false);  //Exposure fusion can be used only with OpenCV3
+		bool exposureFusion = false,         //Exposure fusion can be used only with OpenCV3
+		const ProgressState * state = 0);
 
 
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(

corelib/src/util3d_surface.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UDirectory.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UMath.h"
+#include "rtabmap/utilite/UTimer.h"
 #include <pcl/search/kdtree.h>
 #include <pcl/surface/gp3.h>
 #include <pcl/features/normal_3d_omp.h>
@@ -1076,7 +1077,8 @@ std::vector<cv::Mat> mergeTextures(
 		int blendingDecimation,
 		int brightnessContrastRatioLow,
 		int brightnessContrastRatioHigh,
-		bool exposureFusion)
+		bool exposureFusion,
+		const ProgressState * state)
 {
 	//get texture size, if disabled use default 1024
 	UASSERT(textureSize%256 == 0);
@@ -1315,8 +1317,18 @@ std::vector<cv::Mat> mergeTextures(
 						}
 						++oi;
 					}
+
+					if(state)
+					{
+						if(state->isCanceled())
+						{
+							return std::vector<cv::Mat>();
+						}
+						state->callback(uFormat("Assembled texture %d/%d.", t+1, (int)textures.size()));
+					}
 				}
 
+				UTimer timer;
 				if(vertexToPixels.size())
 				{
 					//UWARN("Saving original.png", globalTexture);
@@ -1479,6 +1491,7 @@ std::vector<cv::Mat> mergeTextures(
 						}
 						//UWARN("Saving gain.png", globalTexture);
 						//cv::imwrite("gain.png", globalTexture);
+						if(state) state->callback(uFormat("Gain compensation %fs", timer.ticks()));
 					}
 
 					if(blending)
@@ -1637,6 +1650,8 @@ std::vector<cv::Mat> mergeTextures(
 							//UWARN("Saving blending.png", globalTexture);
 							//cv::imwrite("blending.png", globalTexture);
 						}
+
+						if(state) state->callback(uFormat("Blending (decimation=%d) %fs", decimation, timer.ticks()));
 					}
 				}
 
@@ -1676,6 +1691,7 @@ std::vector<cv::Mat> mergeTextures(
 								(float)brightnessContrastRatioHigh);
 						}
 					}
+					if(state) state->callback(uFormat("Brightness and contrast auto %fs", timer.ticks()));
 				}
 			}
 		}