diff --git a/app/android/AndroidManifest.xml.in b/app/android/AndroidManifest.xml.in index b6b6cb3f..158f4eb3 100644 --- a/app/android/AndroidManifest.xml.in +++ b/app/android/AndroidManifest.xml.in @@ -2,7 +2,7 @@ diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 0e1c8aec..08722420 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -1843,7 +1843,15 @@ void RTABMapApp::save(const std::string & databasePath) } -cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) const +double sqr(uchar v) +{ + return double(v)*double(v); +} + +cv::Mat RTABMapApp::mergeTextures( + pcl::TextureMesh & mesh, + int textureSize, + const std::vector > & vertexToPixels) const { UASSERT(textureSize> 0); LOGD("textureSize = %d materials=%d", textureSize, mesh.tex_materials.size()); @@ -1893,8 +1901,9 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons // 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)); - bool gainApplied = false; int oi=0; + std::vector imageOrigin(textures.size()); + std::vector newCamIndex(textures.size(), -1); for(int i=0; i<(int)textures.size(); ++i) { if(materialsKept.at(i)) @@ -1903,6 +1912,9 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons int v = oi/cols * 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]); @@ -1912,8 +1924,9 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons UASSERT(!image.empty()); cv::Mat resizedImage; cv::resize(image, resizedImage, emptyImage.size(), 0.0f, 0.0f, cv::INTER_AREA); - if(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)) + 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 channels; cv::split(resizedImage, channels); @@ -1922,7 +1935,6 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons 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); - gainApplied = true; } if(resizedImage.type() == CV_8UC1) { @@ -1948,9 +1960,257 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons progressionStatus_.increment(); } - if(gainApplied) + + if(vertexToPixels.size()) { - rtabmap::util2d::brightnessAndContrastAuto(globalTexture, globalTextureMask, 0.0f, 10.0f); + // gain compensation + const int num_images = static_cast(oi); + cv::Mat_ N(num_images, num_images); N.setTo(0); + cv::Mat_ I(num_images, num_images); I.setTo(0); + + cv::Mat_ IR(num_images, num_images); IR.setTo(0); + cv::Mat_ IG(num_images, num_images); IG.setTo(0); + cv::Mat_ IB(num_images, num_images); IB.setTo(0); + + // Adjust UV coordinates to globalTexture + for(unsigned int p=0; p::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::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; + + // 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 = globalTexture.ptr(vi,ui); + cv::Vec3b * pt2 = globalTexture.ptr(vj,uj); + + I(i, j) += std::sqrt(static_cast(sqr(pt1->val[0]) + sqr(pt1->val[1]) + sqr(pt1->val[2]))); + I(j, i) += std::sqrt(static_cast(sqr(pt2->val[0]) + sqr(pt2->val[1]) + sqr(pt2->val[2]))); + + IR(i, j) += static_cast(pt1->val[2]); + IR(j, i) += static_cast(pt2->val[2]); + IG(i, j) += static_cast(pt1->val[1]); + IG(j, i) += static_cast(pt2->val[1]); + IB(i, j) += static_cast(pt1->val[0]); + IB(j, i) += static_cast(pt2->val[0]); + } + } + } + } + } + + for(int i=0; i A(num_images, num_images); A.setTo(0); + cv::Mat_ b(num_images, 1); b.setTo(0); + cv::Mat_ AR(num_images, num_images); AR.setTo(0); + cv::Mat_ AG(num_images, num_images); AG.setTo(0); + cv::Mat_ 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_ 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_ 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; + + cv::Mat roi = globalTexture(cv::Rect(u, v, emptyImage.cols, emptyImage.rows)); + + std::vector 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 + int decimation = 0; + + // determinate decimation to apply + std::vector 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= edgeLength) + { + decimation = 1 << i; + break; + } + } + } + } + + if(decimation>0) + { + cv::Mat blendGains(globalTexture.rows/decimation, globalTexture.cols/decimation, CV_32FC3, cv::Scalar::all(1.0f)); + for(unsigned int p=0; p 1) + { + std::vector gainsB(vertexToPixels[p].size()); + std::vector gainsG(vertexToPixels[p].size()); + std::vector gainsR(vertexToPixels[p].size()); + float sumWeight = 0.0f; + int k=0; + for(std::map::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; + } + cv::Vec3b * pt = globalTexture.ptr(v,u); + gainsB[k] = static_cast(pt->val[0]) * weight; + gainsG[k] = static_cast(pt->val[1]) * weight; + gainsR[k] = static_cast(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::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; + cv::Vec3b * pt = globalTexture.ptr(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.ptr(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; + } + } + } + } + } + + cv::Mat dst; + cv::blur(blendGains, dst, cv::Size(3,3)); + cv::resize(dst, blendGains, globalTexture.size(), 0, 0, cv::INTER_LINEAR); + + cv::multiply(globalTexture, blendGains, globalTexture, 1.0, CV_8UC3); + } + progressionStatus_.increment(); } } else @@ -2018,9 +2278,15 @@ bool RTABMapApp::exportMesh( if(optimized) { totalSteps += poses.size(); // meshing - if(textureSize > 0 && optimizedMaxPolygons > 0) + if(textureSize > 0) { - totalSteps += 1; // decimation + totalSteps += 1; // gain + totalSteps += 1; // blending + + if(optimizedMaxPolygons > 0) + { + totalSteps += 1; // decimation + } } totalSteps += 1; // texture/coloring @@ -2044,6 +2310,7 @@ bool RTABMapApp::exportMesh( { pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh); pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh); + std::vector > vertexToPixels; cv::Mat globalTexture; int totalPolygons = 0; { @@ -2426,7 +2693,8 @@ bool RTABMapApp::exportMesh( cameraModels, optimizedMaxTextureDistance, optimizedMinTextureClusterSize, - &progressionStatus_); + &progressionStatus_, + &vertexToPixels); LOGI("Texturing... done! %fs", timer.ticks()); if(progressionStatus_.isCanceled()) @@ -2722,7 +2990,7 @@ bool RTABMapApp::exportMesh( if(textureSize>0 && totalPolygons && textureMesh->tex_materials.size()) { LOGI("Merging %d textures...", (int)textureMesh->tex_materials.size()); - globalTexture = mergeTextures(*textureMesh, textureSize); + globalTexture = mergeTextures(*textureMesh, textureSize, vertexToPixels); if(progressionStatus_.isCanceled()) { diff --git a/app/android/jni/RTABMapApp.h b/app/android/jni/RTABMapApp.h index 2d1e9438..0fff4f40 100644 --- a/app/android/jni/RTABMapApp.h +++ b/app/android/jni/RTABMapApp.h @@ -145,7 +145,9 @@ class RTABMapApp : public UEventsHandler { void resetMapping(); void save(const std::string & databasePath); - cv::Mat mergeTextures(pcl::TextureMesh & mesh, int textureSize) const; + cv::Mat mergeTextures(pcl::TextureMesh & mesh, + int textureSize, + const std::vector > & vertexToPixels) const; void cancelProcessing(); bool exportMesh( const std::string & filePath, diff --git a/app/android/jni/scene.cpp b/app/android/jni/scene.cpp index 26c845f7..7c7b0e26 100644 --- a/app/android/jni/scene.cpp +++ b/app/android/jni/scene.cpp @@ -175,7 +175,7 @@ Scene::Scene() : pointSize_(5.0f), frustumCulling_(true), boundingBoxRendering_(false), - lighting_(true), + lighting_(false), backfaceCulling_(true), r_(0.0f), g_(0.0f), diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java index 309d5818..20b2af1d 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java @@ -255,7 +255,7 @@ public class RTABMapActivity extends Activity implements OnClickListener { mButtonSaveOnDevice.setOnClickListener(this); mButtonShareOnSketchfab.setOnClickListener(this); mButtonFirst.setChecked(true); - mButtonLighting.setChecked(true); + mButtonLighting.setChecked(false); mButtonLighting.setVisibility(View.INVISIBLE); mButtonCloseVisualization.setVisibility(View.INVISIBLE); mButtonSaveOnDevice.setVisibility(View.INVISIBLE);