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

Tango: refactored mesh creation

Browse Source
This commit is contained in:
matlabbe
2017-06-16 14:44:48 -04:00
parent a1f2f95d1b
commit 8787ecceb8
5 changed files with 131 additions and 308 deletions
Download Patch File Download Diff File Expand all files Collapse all files

101
corelib/include/rtabmap/core/impl/util3d_surface.hpp
View File

@@ -48,7 +48,6 @@ std::vector<pcl::Vertices> normalizePolygonsSide(
template<typename pointRGBT>
void denseMeshPostProcessing(
pcl::PolygonMeshPtr & mesh,
bool hasColors,
float meshDecimationFactor,
int maximumPolygons,
const typename pcl::PointCloud<pointRGBT>::Ptr & cloud,
@@ -58,6 +57,21 @@ void denseMeshPostProcessing(
int minClusterSize,
ProgressState * progressState)
{
// compute normals for the mesh if not already here
bool hasNormals = false;
bool hasColors = false;
for(unsigned int i=0; i<mesh->cloud.fields.size(); ++i)
{
if(mesh->cloud.fields[i].name.compare("normal_x") == 0)
{
hasNormals = true;
}
else if(mesh->cloud.fields[i].name.compare("rgb") == 0)
{
hasColors = true;
}
}
if(maximumPolygons > 0)
{
double factor = 1.0-double(maximumPolygons)/double(mesh->polygons.size());
@@ -77,6 +91,7 @@ void denseMeshPostProcessing(
{
if(progressState) progressState->callback(uFormat("Decimated mesh has more polygons than before!"));
}
hasNormals = false;
hasColors = false;
}
@@ -90,7 +105,7 @@ void denseMeshPostProcessing(
// transfer color from point cloud to mesh
typename pcl::search::KdTree<pointRGBT>::Ptr tree (new pcl::search::KdTree<pointRGBT>(true));
tree->setInputCloud(cloud);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr coloredCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr coloredCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::fromPCLPointCloud2(mesh->cloud, *coloredCloud);
std::vector<bool> coloredPts(coloredCloud->size());
for(unsigned int i=0; i<coloredCloud->size(); ++i)
@@ -162,6 +177,7 @@ void denseMeshPostProcessing(
filteredPolygons.resize(oi);
mesh->polygons = filteredPolygons;
}
hasColors = true;
}
else if(cloud.get()!=0 &&
!hasColors &&
@@ -174,7 +190,7 @@ void denseMeshPostProcessing(
// transfer color from point cloud to mesh
typename pcl::search::KdTree<pointRGBT>::Ptr tree (new pcl::search::KdTree<pointRGBT>(true));
tree->setInputCloud(cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr optimizedCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointNormal>::Ptr optimizedCloud(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromPCLPointCloud2(mesh->cloud, *optimizedCloud);
std::vector<bool> closePts(optimizedCloud->size());
for(unsigned int i=0; i<optimizedCloud->size(); ++i)
@@ -276,6 +292,85 @@ void denseMeshPostProcessing(
if(progressState) progressState->callback(uFormat("Filtered %1 polygons.", before-(int)mesh->polygons.size()));
}
// compute normals for the mesh if not already here, add also white color if colored output is required
if(!hasNormals || (!hasColors && coloredOutput))
{
// use polygons
if(hasColors || coloredOutput)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
Eigen::Vector3f normal(1,0,0);
for(unsigned int i=0; i<mesh->polygons.size(); ++i)
{
pcl::Vertices & v = mesh->polygons[i];
if(!hasNormals)
{
UASSERT(v.vertices.size()>2);
Eigen::Vector3f v0(
cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
int last = v.vertices.size()-1;
Eigen::Vector3f v1(
cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
normal = v0.cross(v1);
normal.normalize();
}
// flat normal (per face)
for(unsigned int j=0; j<v.vertices.size(); ++j)
{
if(!hasNormals)
{
cloud->at(v.vertices[j]).normal_x = normal[0];
cloud->at(v.vertices[j]).normal_y = normal[1];
cloud->at(v.vertices[j]).normal_z = normal[2];
}
if(!hasColors)
{
cloud->at(v.vertices[j]).r = 255;
cloud->at(v.vertices[j]).g = 255;
cloud->at(v.vertices[j]).b = 255;
}
}
}
pcl::toPCLPointCloud2 (*cloud, mesh->cloud);
}
else
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
for(unsigned int i=0; i<mesh->polygons.size(); ++i)
{
pcl::Vertices & v = mesh->polygons[i];
UASSERT(v.vertices.size()>2);
Eigen::Vector3f v0(
cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
int last = v.vertices.size()-1;
Eigen::Vector3f v1(
cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
Eigen::Vector3f normal = v0.cross(v1);
normal.normalize();
// flat normal (per face)
for(unsigned int j=0; j<v.vertices.size(); ++j)
{
cloud->at(v.vertices[j]).normal_x = normal[0];
cloud->at(v.vertices[j]).normal_y = normal[1];
cloud->at(v.vertices[j]).normal_z = normal[2];
}
}
pcl::toPCLPointCloud2 (*cloud, mesh->cloud);
}
}
}
}