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

Tango: Added Rendering Options... -> Mesh Rendering -> Mesh Decimation

Browse Source
This commit is contained in:
matlabbe
2016-12-03 21:49:40 -05:00
parent d2287ab9f4
commit 3c17049084
15 changed files with 365 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -133,6 +133,7 @@ RTABMapApp::RTABMapApp() :
fullResolution_(false),
appendMode_(true),
maxCloudDepth_(0.0),
meshDecimation_(1),
meshTrianglePix_(1),
meshAngleToleranceDeg_(15.0),
paused_(false),
@@ -333,35 +334,58 @@ bool RTABMapApp::smoothMesh(int id, Mesh & mesh)
{
UTimer t;
// reconstruct depth image
cv::Mat depth = cv::Mat::zeros(mesh.height, mesh.width, CV_32FC1);
UASSERT(mesh.indices.get() && mesh.indices->size());
cv::Mat depth = cv::Mat::zeros(mesh.cloud->height, mesh.cloud->width, CV_32FC1);
rtabmap::Transform localTransformInv = mesh.cameraModel.localTransform().inverse();
for(unsigned int i=0; i<mesh.denseToOrganizedIndices.size(); ++i)
for(unsigned int i=0; i<mesh.indices->size(); ++i)
{
int index = mesh.indices->at(i);
// FastBilateralFilter works in camera frame
pcl::PointXYZRGB pt = rtabmap::util3d::transformPoint(mesh.cloud->at(i), localTransformInv);
depth.at<float>(mesh.denseToOrganizedIndices[i]) = pt.z;
if(mesh.cloud->at(index).x > 0)
{
pcl::PointXYZRGB pt = rtabmap::util3d::transformPoint(mesh.cloud->at(index), localTransformInv);
depth.at<float>(index) = pt.z;
}
}
depth = rtabmap::util2d::fastBilateralFiltering(depth, 2.0f, 0.075f);
LOGI("smoothMesh() Bilateral filtering of %d, time=%fs", id, t.ticks());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudOrganized = rtabmap::util3d::cloudFromDepthRGB(mesh.texture.rows>1?mesh.texture:rtabmap::uncompressImage(mesh.texture), depth, mesh.cameraModel, 1, maxCloudDepth_);
cloudOrganized = rtabmap::util3d::transformPointCloud(cloudOrganized, mesh.cameraModel.localTransform());
//reconstruct the mesh with smoothed surfaces
std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloudOrganized, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
// filter NaN points
pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> outputPolygons;
std::vector<int> denseToOrganizedIndices = rtabmap::util3d::filterNaNPointsFromMesh(*cloudOrganized, polygons, *outputCloud, outputPolygons);
LOGI("smoothMesh() Reconstructing the mesh of %d, time=%fs", id, t.ticks());
if(outputPolygons.size())
if(!depth.empty() && mesh.indices->size())
{
mesh.cloud = outputCloud;
mesh.polygons = outputPolygons;
mesh.denseToOrganizedIndices = denseToOrganizedIndices;
pcl::IndicesPtr newIndices(new std::vector<int>(mesh.indices->size()));
int oi = 0;
for(unsigned int i=0; i<mesh.indices->size(); ++i)
{
int index = mesh.indices->at(i);
pcl::PointXYZRGB & pt = mesh.cloud->at(index);
pcl::PointXYZRGB newPt = rtabmap::util3d::transformPoint(mesh.cloud->at(index), localTransformInv);
if(depth.at<float>(index) > 0)
{
newPt.z = depth.at<float>(index);
newPt = rtabmap::util3d::transformPoint(newPt, mesh.cameraModel.localTransform());
newIndices->at(oi++) = index;
}
else
{
newPt.x = newPt.y = newPt.z = std::numeric_limits<float>::quiet_NaN();
}
pt.x = newPt.x;
pt.y = newPt.y;
pt.z = newPt.z;
}
newIndices->resize(oi);
mesh.indices = newIndices;
//reconstruct the mesh with smoothed surfaces
std::vector<pcl::Vertices> polygons;
if(main_scene_.isMeshRendering())
{
polygons = rtabmap::util3d::organizedFastMesh(mesh.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
}
LOGI("smoothMesh() Reconstructing the mesh of %d, time=%fs", id, t.ticks());
mesh.polygons = polygons;
}
else
{
@@ -414,11 +438,13 @@ int RTABMapApp::Render()
{
if(!main_scene_.hasCloud(iter->first))
{
cv::Mat compressed = iter->second.texture;
iter->second.texture = rtabmap::uncompressImage(iter->second.texture);
main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose);
cv::Mat texture;
if(main_scene_.isMeshTexturing())
{
texture = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(iter->first));
}
main_scene_.addMesh(iter->first, iter->second, texture, opengl_world_T_rtabmap_world*iter->second.pose);
main_scene_.setCloudVisible(iter->first, iter->second.visible);
iter->second.texture = compressed;
}
}
}
@@ -554,56 +580,33 @@ int RTABMapApp::Render()
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
LOGI("Creating node cloud %d (depth=%dx%d rgb=%dx%d)", id, data.depthRaw().cols, data.depthRaw().rows, data.imageRaw().cols, data.imageRaw().rows);
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, 0, indices.get());
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get());
if(cloud->size() && indices->size())
{
UTimer time;
// pcl::organizedFastMesh doesn't take indices, so set to NaN points we don't need to mesh
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::ExtractIndices<pcl::PointXYZRGB> filter;
filter.setIndices(indices);
filter.setKeepOrganized(true);
filter.setInputCloud(cloud);
filter.filter(*output);
std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> outputPolygons;
std::vector<int> denseToOrganizedIndices = rtabmap::util3d::filterNaNPointsFromMesh(*output, polygons, *outputCloud, outputPolygons);
LOGI("Creating mesh, %d polygons (%fs)", (int)outputPolygons.size(), time.ticks());
if(outputCloud->size() && outputPolygons.size())
std::vector<pcl::Vertices> polygons;
if(main_scene_.isMeshRendering())
{
totalPolygons_ += outputPolygons.size();
polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
LOGI("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
}
if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering())
{
totalPolygons_ += polygons.size();
std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
UASSERT(inserted.second);
inserted.first->second.cloud = outputCloud;
inserted.first->second.denseToOrganizedIndices = denseToOrganizedIndices;
inserted.first->second.width = cloud->width;
inserted.first->second.height = cloud->height;
inserted.first->second.polygons = outputPolygons;
inserted.first->second.cloud = cloud;
inserted.first->second.indices = indices;
inserted.first->second.polygons = polygons;
inserted.first->second.pose = opengl_world_T_rtabmap_world.inverse()*iter->second;
inserted.first->second.visible = true;
inserted.first->second.texture = data.imageRaw();
inserted.first->second.cameraModel = data.cameraModels()[0];
inserted.first->second.gain = 1.0f;
if(notifyDataLoaded)
{
// gain compensation is done, so don't compress the texture yet
inserted.first->second.texture = data.imageRaw(); // keep raw
// mesh will be added in gain compensation below
}
else
{
main_scene_.addMesh(id, inserted.first->second, iter->second);
inserted.first->second.texture = data.imageCompressed(); // keep compressed
}
main_scene_.addMesh(id, inserted.first->second, main_scene_.isMeshTexturing()?data.imageRaw():cv::Mat(), iter->second);
}
else
{
@@ -646,7 +649,7 @@ int RTABMapApp::Render()
main_scene_.setCloudVisible(*iter, false);
std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(*iter);
UASSERT(meshIter!=createdMeshes_.end());
meshIter->second.visible = true;
meshIter->second.visible = false;
}
}
}
@@ -663,14 +666,15 @@ int RTABMapApp::Render()
if(!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), 1, maxCloudDepth_);
pcl::IndicesPtr indices(new std::vector<int>);
cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation_, maxCloudDepth_, 0.0f, indices.get());
if(cloud->size())
{
LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows,
odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows,
(int)cloud->width, (int)cloud->height);
main_scene_.addCloud(-1, cloud, opengl_world_T_rtabmap_world*odomEvent.pose());
main_scene_.addCloud(-1, cloud, indices, opengl_world_T_rtabmap_world*odomEvent.pose());
main_scene_.setCloudVisible(-1, true);
}
else
@@ -692,11 +696,12 @@ int RTABMapApp::Render()
LOGI("Gain compensation...");
boost::mutex::scoped_lock lock(meshesMutex_);
rtabmap::GainCompensator compensator;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds;
std::map<int, pcl::IndicesPtr> indices;
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
clouds.insert(std::make_pair(iter->first, iter->second.cloud));
indices.insert(std::make_pair(iter->first, iter->second.indices));
}
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
@@ -718,38 +723,24 @@ int RTABMapApp::Render()
}
}
rtabmap::GainCompensator compensator;
if(clouds.size() > 1 && links.size())
{
compensator.feed(clouds, links);
compensator.feed(clouds, indices, links);
LOGI("Gain compensation... compute gain: links=%d, time=%fs", (int)links.size(), tGainCompensation.ticks());
}
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
cv::Mat compressedImage = iter->second.texture;
iter->second.texture = compressedImage.rows == 1 ? rtabmap::uncompressImage(compressedImage) : compressedImage;
if(!iter->second.cloud->empty())
{
if(clouds.size() > 1 && links.size())
{
compensator.apply(iter->first, iter->second.cloud);
if(!iter->second.texture.empty())
{
compensator.apply(iter->first, iter->second.texture);
}
iter->second.gain = compensator.getGain(iter->first);
}
}
if(notifyDataLoaded)
{
main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world* poses.at(iter->first));
}
else
{
main_scene_.updateMesh(iter->first, iter->second);
}
iter->second.texture = rtabmap::compressImage2(iter->second.texture, ".jpg");
main_scene_.updateMesh(iter->first, iter->second, cv::Mat());
}
LOGI("Gain compensation... applying gain: meshes=%d, time=%fs", (int)createdMeshes_.size(), tGainCompensation.ticks());
@@ -764,11 +755,11 @@ int RTABMapApp::Render()
boost::mutex::scoped_lock lock(meshesMutex_);
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
if(iter->second.cloud->size())
if(iter->second.cloud->size() && iter->second.indices->size())
{
if(smoothMesh(iter->first, iter->second))
{
main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose);
main_scene_.updateMesh(iter->first, iter->second, cv::Mat());
}
}
}
@@ -980,6 +971,45 @@ void RTABMapApp::setMaxCloudDepth(float value)
maxCloudDepth_ = value;
}
void RTABMapApp::setMeshDecimation(int value)
{
LOGE("Set decimation to level %d", value);
meshDecimation_ = 1;
if(camera_)
{
// Google Tango Tablet 160x90
// Phab2Pro 240x135
int width = camera_->getCameraModel().imageWidth()/8;
if(value == 2) // high
{
if(width % 10 == 0)
{
meshDecimation_ = 10;
}
else if(width % 15 == 0)
{
meshDecimation_ = 15;
}
else
{
LOGE("Could not set decimation to high (width=%d)", width);
}
}
else if(value == 1) // medium
{
if(width % 5 == 0)
{
meshDecimation_ = 5;
}
else
{
LOGE("Could not set decimation to medium (width=%d)", width);
}
}
}
LOGE("Set decimation to %d", meshDecimation_);
}
void RTABMapApp::setMeshAngleTolerance(float value)
{
meshAngleToleranceDeg_ = value;
@@ -1049,13 +1079,26 @@ void RTABMapApp::save(const std::string & databasePath)
{
rtabmapThread_->join(true);
// save mapping parameters in the database
bool appendModeBackup = appendMode_;
if(appendMode_)
{
appendMode_ = false;
}
bool dataRecorderModeBackup = dataRecorderMode_;
if(dataRecorderMode_)
{
// to save mapping parameters in the database
dataRecorderMode_ = false;
}
if(appendModeBackup || dataRecorderModeBackup)
{
rtabmap::ParametersMap parameters = getRtabmapParameters();
rtabmap_->parseParameters(parameters);
dataRecorderMode_ = true;
appendMode_ = appendModeBackup;
dataRecorderMode_ = dataRecorderModeBackup;
}
rtabmap_->close(true, databasePath);
@@ -1076,7 +1119,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
if(UFile::getExtension(filePath).compare("obj") == 0)
{
pcl::TextureMesh textureMesh;
std::vector<cv::Mat> textures;
std::vector<int> textures;
int totalPolygons = 0;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
{
@@ -1093,42 +1136,57 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
iter!= createdMeshes_.end();
++iter)
{
if(!iter->second.texture.empty() &&
if(!rtabmap_->getMemory()->getImageCompressed(iter->first).empty() &&
iter->second.cloud->size() &&
iter->second.polygons.size() &&
(!iter->second.cloud->is_dense || (iter->second.cloud->is_dense && iter->second.denseToOrganizedIndices.size() == iter->second.cloud->size())))
iter->second.polygons.size())
{
// Convert organized to dense cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> outputPolygons;
std::vector<int> denseToOrganizedIndices = rtabmap::util3d::filterNaNPointsFromMesh(*iter->second.cloud, iter->second.polygons, *outputCloud, outputPolygons);
if(iter->second.gain != 1.0f)
{
for(unsigned int i=0; i<outputCloud->size(); ++i)
{
pcl::PointXYZRGB & pt = outputCloud->at(i);
pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * iter->second.gain)));
pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * iter->second.gain)));
pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * iter->second.gain)));
}
}
// OBJ format requires normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(iter->second.cloud, 6);
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(outputCloud, 6);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*iter->second.cloud, *normals, *cloudWithNormals);
pcl::concatenateFields(*outputCloud, *normals, *cloudWithNormals);
// polygons
UASSERT(iter->second.polygons.size());
unsigned int polygonSize = iter->second.polygons.front().vertices.size();
textureMesh.tex_polygons[oi].resize(iter->second.polygons.size());
textureMesh.tex_coordinates[oi].resize(iter->second.polygons.size() * polygonSize);
for(unsigned int j=0; j<iter->second.polygons.size(); ++j)
UASSERT(outputPolygons.size());
unsigned int polygonSize = outputPolygons.front().vertices.size();
textureMesh.tex_polygons[oi].resize(outputPolygons.size());
textureMesh.tex_coordinates[oi].resize(outputPolygons.size() * polygonSize);
for(unsigned int j=0; j<outputPolygons.size(); ++j)
{
pcl::Vertices vertices = iter->second.polygons[j];
pcl::Vertices vertices = outputPolygons[j];
UASSERT(polygonSize == vertices.vertices.size());
for(unsigned int k=0; k<vertices.vertices.size(); ++k)
{
//uv
UASSERT(vertices.vertices[k] < iter->second.denseToOrganizedIndices.size());
int originalVertex = iter->second.denseToOrganizedIndices[vertices.vertices[k]];
UASSERT(vertices.vertices[k] < denseToOrganizedIndices.size());
int originalVertex = denseToOrganizedIndices[vertices.vertices[k]];
textureMesh.tex_coordinates[oi][j*vertices.vertices.size()+k] = Eigen::Vector2f(
float(originalVertex % iter->second.width) / float(iter->second.width), // u
float(iter->second.height - originalVertex / iter->second.width) / float(iter->second.height)); // v
float(originalVertex % iter->second.cloud->width) / float(iter->second.cloud->width), // u
float(iter->second.cloud->height - originalVertex / iter->second.cloud->width) / float(iter->second.cloud->height)); // v
vertices.vertices[k] += polygonsStep;
}
textureMesh.tex_polygons[oi][j] = vertices;
}
totalPolygons += iter->second.polygons.size();
polygonsStep += iter->second.cloud->size();
totalPolygons += outputPolygons.size();
polygonsStep += outputCloud->size();
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(cloudWithNormals, iter->second.pose);
if(mergedClouds->size() == 0)
@@ -1140,7 +1198,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
*mergedClouds += *transformedCloud;
}
textures[oi] = iter->second.texture;
textures[oi] = iter->first;
textureMesh.tex_materials[oi].tex_illum = 1;
textureMesh.tex_materials[oi].tex_name = uFormat("material_%d", iter->first);
++oi;
@@ -1163,7 +1221,11 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
UDirectory::makeDir(textureDirectory);
for(unsigned int i=0;i<textures.size(); ++i)
{
cv::Mat rawImage = textures[i].rows>1?textures[i]:rtabmap::uncompressImage(textures[i]);
cv::Mat rawImage = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(textures[i]));
if(createdMeshes_.at(textures[i]).gain != 1.0f)
{
cv::multiply(rawImage, createdMeshes_.at(textures[i]).gain, rawImage);
}
std::string texFile = textureDirectory+"/"+textureMesh.tex_materials[i].tex_name+".png";
cv::imwrite(texFile, rawImage);
@@ -1198,20 +1260,36 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
iter!= createdMeshes_.end();
++iter)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(iter->second.cloud, iter->second.pose);
// Convert organized to dense cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> outputPolygons;
rtabmap::util3d::filterNaNPointsFromMesh(*iter->second.cloud, iter->second.polygons, *outputCloud, outputPolygons);
if(iter->second.gain != 1.0f)
{
for(unsigned int i=0; i<outputCloud->size(); ++i)
{
pcl::PointXYZRGB & pt = outputCloud->at(i);
pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * iter->second.gain)));
pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * iter->second.gain)));
pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * iter->second.gain)));
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(outputCloud, iter->second.pose);
if(mergedClouds->size() == 0)
{
*mergedClouds = *transformedCloud;
mergedPolygons = iter->second.polygons;
mergedPolygons = outputPolygons;
}
else
{
rtabmap::util3d::appendMesh(*mergedClouds, mergedPolygons, *transformedCloud, iter->second.polygons);
rtabmap::util3d::appendMesh(*mergedClouds, mergedPolygons, *transformedCloud, outputPolygons);
}
}
}
if(mergedClouds->size() && mergedPolygons.size())
if(mergedClouds->size())
{
pcl::PolygonMesh mesh;
pcl::toPCLPointCloud2(*mergedClouds, mesh.cloud);