i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
matlabbe
2017-06-14 10:33:26 -04:00
parent 811afa1171
commit ba4437edee
5 changed files with 31 additions and 427 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
app/android/jni/ProgressionStatus.h
View File

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

431
app/android/jni/RTABMapApp.cpp
View File

@@ -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())
{

4
app/android/jni/RTABMapApp.h
View File

@@ -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,