mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
Tango: refactored mesh creation
This commit is contained in:
@@ -2121,9 +2121,11 @@ bool RTABMapApp::exportMesh(
|
|||||||
|
|
||||||
if(mesh->polygons.size())
|
if(mesh->polygons.size())
|
||||||
{
|
{
|
||||||
if(textureSize > 0 && optimizedMaxPolygons > 0 && optimizedMaxPolygons < (int)mesh->polygons.size())
|
totalPolygons=(int)mesh->polygons.size();
|
||||||
|
|
||||||
|
if(optimizedMaxPolygons > 0 && optimizedMaxPolygons < (int)mesh->polygons.size())
|
||||||
{
|
{
|
||||||
#ifndef DISABLE_VTK
|
#ifndef DISABLE_VTK
|
||||||
unsigned int count = mesh->polygons.size();
|
unsigned int count = mesh->polygons.size();
|
||||||
float factor = 1.0f-float(optimizedMaxPolygons)/float(count);
|
float factor = 1.0f-float(optimizedMaxPolygons)/float(count);
|
||||||
LOGI("Mesh decimation (max polygons %d/%d -> factor=%f)...", optimizedMaxPolygons, (int)count, factor);
|
LOGI("Mesh decimation (max polygons %d/%d -> factor=%f)...", optimizedMaxPolygons, (int)count, factor);
|
||||||
@@ -2147,9 +2149,9 @@ bool RTABMapApp::exportMesh(
|
|||||||
{
|
{
|
||||||
UWARN("Decimated mesh has more polygons than before!");
|
UWARN("Decimated mesh has more polygons than before!");
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
UWARN("RTAB-Map is not built with PCL-VTK module so mesh decimation cannot be used!");
|
UWARN("RTAB-Map is not built with PCL-VTK module so mesh decimation cannot be used!");
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
if(progressionStatus_.isCanceled())
|
if(progressionStatus_.isCanceled())
|
||||||
@@ -2164,206 +2166,18 @@ bool RTABMapApp::exportMesh(
|
|||||||
|
|
||||||
progressionStatus_.increment();
|
progressionStatus_.increment();
|
||||||
|
|
||||||
if(textureSize == 0)
|
rtabmap::util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
|
||||||
|
mesh,
|
||||||
|
0.0f,
|
||||||
|
0,
|
||||||
|
mergedClouds,
|
||||||
|
optimizedColorRadius,
|
||||||
|
textureSize == 0,
|
||||||
|
optimizedCleanWhitePolygons,
|
||||||
|
0);
|
||||||
|
|
||||||
|
if(textureSize>0)
|
||||||
{
|
{
|
||||||
// colored polygon mesh
|
|
||||||
if(optimizedColorRadius >= 0.0f)
|
|
||||||
{
|
|
||||||
LOGI("Transferring color from point cloud to mesh...");
|
|
||||||
// transfer color from point cloud to mesh
|
|
||||||
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>(true));
|
|
||||||
tree->setInputCloud(mergedClouds);
|
|
||||||
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)
|
|
||||||
{
|
|
||||||
std::vector<int> kIndices;
|
|
||||||
std::vector<float> kDistances;
|
|
||||||
pcl::PointXYZRGBNormal pt;
|
|
||||||
pt.x = coloredCloud->at(i).x;
|
|
||||||
pt.y = coloredCloud->at(i).y;
|
|
||||||
pt.z = coloredCloud->at(i).z;
|
|
||||||
if(optimizedColorRadius > 0.0f)
|
|
||||||
{
|
|
||||||
tree->radiusSearch(pt, optimizedColorRadius, kIndices, kDistances);
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
tree->nearestKSearch(pt, 1, kIndices, kDistances);
|
|
||||||
}
|
|
||||||
if(kIndices.size())
|
|
||||||
{
|
|
||||||
//compute average color
|
|
||||||
int r=0;
|
|
||||||
int g=0;
|
|
||||||
int b=0;
|
|
||||||
int a=0;
|
|
||||||
for(unsigned int j=0; j<kIndices.size(); ++j)
|
|
||||||
{
|
|
||||||
r+=(int)mergedClouds->at(kIndices[j]).r;
|
|
||||||
g+=(int)mergedClouds->at(kIndices[j]).g;
|
|
||||||
b+=(int)mergedClouds->at(kIndices[j]).b;
|
|
||||||
a+=(int)mergedClouds->at(kIndices[j]).a;
|
|
||||||
}
|
|
||||||
coloredCloud->at(i).r = r/kIndices.size();
|
|
||||||
coloredCloud->at(i).g = g/kIndices.size();
|
|
||||||
coloredCloud->at(i).b = b/kIndices.size();
|
|
||||||
coloredCloud->at(i).a = a/kIndices.size();
|
|
||||||
coloredPts.at(i) = true;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
//white
|
|
||||||
coloredCloud->at(i).r = coloredCloud->at(i).g = coloredCloud->at(i).b = 255;
|
|
||||||
coloredPts.at(i) = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// recompute normals and remove polygons with no color
|
|
||||||
std::vector<pcl::Vertices> filteredPolygons(optimizedCleanWhitePolygons?mesh->polygons.size():0);
|
|
||||||
int oi=0;
|
|
||||||
for(unsigned int i=0; i<mesh->polygons.size(); ++i)
|
|
||||||
{
|
|
||||||
// recompute normals
|
|
||||||
pcl::Vertices & v = mesh->polygons[i];
|
|
||||||
UASSERT(v.vertices.size()>2);
|
|
||||||
Eigen::Vector3f v0(
|
|
||||||
coloredCloud->at(v.vertices[1]).x - coloredCloud->at(v.vertices[0]).x,
|
|
||||||
coloredCloud->at(v.vertices[1]).y - coloredCloud->at(v.vertices[0]).y,
|
|
||||||
coloredCloud->at(v.vertices[1]).z - coloredCloud->at(v.vertices[0]).z);
|
|
||||||
int last = v.vertices.size()-1;
|
|
||||||
Eigen::Vector3f v1(
|
|
||||||
coloredCloud->at(v.vertices[last]).x - coloredCloud->at(v.vertices[0]).x,
|
|
||||||
coloredCloud->at(v.vertices[last]).y - coloredCloud->at(v.vertices[0]).y,
|
|
||||||
coloredCloud->at(v.vertices[last]).z - coloredCloud->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)
|
|
||||||
{
|
|
||||||
coloredCloud->at(v.vertices[j]).normal_x = normal[0];
|
|
||||||
coloredCloud->at(v.vertices[j]).normal_y = normal[1];
|
|
||||||
coloredCloud->at(v.vertices[j]).normal_z = normal[2];
|
|
||||||
}
|
|
||||||
|
|
||||||
if(optimizedCleanWhitePolygons)
|
|
||||||
{
|
|
||||||
bool coloredPolygon = true;
|
|
||||||
for(unsigned int j=0; j<mesh->polygons[i].vertices.size(); ++j)
|
|
||||||
{
|
|
||||||
if(!coloredPts.at(mesh->polygons[i].vertices[j]))
|
|
||||||
{
|
|
||||||
coloredPolygon = false;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if(coloredPolygon)
|
|
||||||
{
|
|
||||||
filteredPolygons[oi++] = mesh->polygons[i];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if(optimizedCleanWhitePolygons)
|
|
||||||
{
|
|
||||||
filteredPolygons.resize(oi);
|
|
||||||
mesh->polygons = filteredPolygons;
|
|
||||||
}
|
|
||||||
|
|
||||||
pcl::toPCLPointCloud2(*coloredCloud, mesh->cloud);
|
|
||||||
LOGI("Transfering color from point cloud to mesh...done! %fs", timer.ticks());
|
|
||||||
}
|
|
||||||
else // recompute normals
|
|
||||||
{
|
|
||||||
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
|
|
||||||
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];
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
polygonMesh = mesh;
|
|
||||||
totalPolygons = mesh->polygons.size();
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
if(optimizedColorRadius > 0.0f && optimizedCleanWhitePolygons)
|
|
||||||
{
|
|
||||||
LOGI("Removing polygons too far from the cloud");
|
|
||||||
// transfer color from point cloud to mesh
|
|
||||||
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>(true));
|
|
||||||
tree->setInputCloud(mergedClouds);
|
|
||||||
pcl::PointCloud<pcl::PointXYZ>::Ptr optimizedCloud(new pcl::PointCloud<pcl::PointXYZ>);
|
|
||||||
pcl::fromPCLPointCloud2(mesh->cloud, *optimizedCloud);
|
|
||||||
std::vector<bool> closePts(optimizedCloud->size());
|
|
||||||
for(unsigned int i=0; i<optimizedCloud->size(); ++i)
|
|
||||||
{
|
|
||||||
std::vector<int> kIndices;
|
|
||||||
std::vector<float> kDistances;
|
|
||||||
pcl::PointXYZRGBNormal pt;
|
|
||||||
pt.x = optimizedCloud->at(i).x;
|
|
||||||
pt.y = optimizedCloud->at(i).y;
|
|
||||||
pt.z = optimizedCloud->at(i).z;
|
|
||||||
tree->radiusSearch(pt, optimizedColorRadius, kIndices, kDistances);
|
|
||||||
if(kIndices.size())
|
|
||||||
{
|
|
||||||
closePts.at(i) = true;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
closePts.at(i) = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// remove far polygons
|
|
||||||
std::vector<pcl::Vertices> filteredPolygons(mesh->polygons.size());
|
|
||||||
int oi=0;
|
|
||||||
for(unsigned int i=0; i<mesh->polygons.size(); ++i)
|
|
||||||
{
|
|
||||||
bool keepPolygon = true;
|
|
||||||
for(unsigned int j=0; j<mesh->polygons[i].vertices.size(); ++j)
|
|
||||||
{
|
|
||||||
if(!closePts.at(mesh->polygons[i].vertices[j]))
|
|
||||||
{
|
|
||||||
keepPolygon = false;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
if(keepPolygon)
|
|
||||||
{
|
|
||||||
filteredPolygons[oi++] = mesh->polygons[i];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
filteredPolygons.resize(oi);
|
|
||||||
mesh->polygons = filteredPolygons;
|
|
||||||
|
|
||||||
LOGI("Removing polygons too far from the cloud...done! %fs", timer.ticks());
|
|
||||||
}
|
|
||||||
|
|
||||||
LOGI("Texturing...");
|
LOGI("Texturing...");
|
||||||
textureMesh = rtabmap::util3d::createTextureMesh(
|
textureMesh = rtabmap::util3d::createTextureMesh(
|
||||||
mesh,
|
mesh,
|
||||||
@@ -2393,109 +2207,21 @@ bool RTABMapApp::exportMesh(
|
|||||||
if(textureMesh->tex_coordinates.size() && optimizedCleanWhitePolygons)
|
if(textureMesh->tex_coordinates.size() && optimizedCleanWhitePolygons)
|
||||||
{
|
{
|
||||||
LOGI("Cleanup mesh...");
|
LOGI("Cleanup mesh...");
|
||||||
|
rtabmap::util3d::cleanTextureMesh(*textureMesh, 0);
|
||||||
// assume last texture is the occluded texture
|
LOGI("Cleanup mesh... done! %fs", timer.ticks());
|
||||||
textureMesh->tex_coordinates.pop_back();
|
|
||||||
textureMesh->tex_polygons.pop_back();
|
|
||||||
textureMesh->tex_materials.pop_back();
|
|
||||||
|
|
||||||
if(clusterRatio_>0.0f)
|
|
||||||
{
|
|
||||||
LOGI("Filter small polygon clusters...");
|
|
||||||
|
|
||||||
// concatenate all polygons
|
|
||||||
int totalSize = 0;
|
|
||||||
for(unsigned int t=0; t<textureMesh->tex_polygons.size(); ++t)
|
|
||||||
{
|
|
||||||
totalSize+=textureMesh->tex_polygons[t].size();
|
|
||||||
}
|
|
||||||
std::vector<pcl::Vertices> allPolygons(totalSize);
|
|
||||||
int oi=0;
|
|
||||||
for(unsigned int t=0; t<textureMesh->tex_polygons.size(); ++t)
|
|
||||||
{
|
|
||||||
for(unsigned int i=0; i<textureMesh->tex_polygons[t].size(); ++i)
|
|
||||||
{
|
|
||||||
allPolygons[oi++] = textureMesh->tex_polygons[t][i];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// filter polygons
|
|
||||||
std::vector<std::set<int> > neighbors;
|
|
||||||
std::vector<std::set<int> > vertexToPolygons;
|
|
||||||
rtabmap::util3d::createPolygonIndexes(allPolygons,
|
|
||||||
textureMesh->cloud.data.size()/textureMesh->cloud.point_step,
|
|
||||||
neighbors,
|
|
||||||
vertexToPolygons);
|
|
||||||
std::list<std::list<int> > clusters = rtabmap::util3d::clusterPolygons(
|
|
||||||
neighbors,
|
|
||||||
optimizedMinTextureClusterSize);
|
|
||||||
|
|
||||||
std::set<int> validPolygons;
|
|
||||||
for(std::list<std::list<int> >::iterator kter=clusters.begin(); kter!=clusters.end(); ++kter)
|
|
||||||
{
|
|
||||||
for(std::list<int>::iterator jter=kter->begin(); jter!=kter->end(); ++jter)
|
|
||||||
{
|
|
||||||
validPolygons.insert(*jter);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// for each texture
|
|
||||||
unsigned int allPolygonsIndex = 0;
|
|
||||||
for(unsigned int t=0; t<textureMesh->tex_polygons.size(); ++t)
|
|
||||||
{
|
|
||||||
std::vector<pcl::Vertices> filteredPolygons(textureMesh->tex_polygons[t].size());
|
|
||||||
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
|
|
||||||
std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > filteredCoordinates(textureMesh->tex_coordinates[t].size());
|
|
||||||
#else
|
|
||||||
std::vector<Eigen::Vector2f> filteredCoordinates(textureMesh->tex_coordinates[t].size());
|
|
||||||
#endif
|
|
||||||
int oi=0;
|
|
||||||
unsigned int polygonSize = 0;
|
|
||||||
if(textureMesh->tex_polygons[t].size())
|
|
||||||
{
|
|
||||||
UASSERT(allPolygonsIndex < allPolygons.size());
|
|
||||||
|
|
||||||
polygonSize = textureMesh->tex_polygons[t][0].vertices.size();
|
|
||||||
|
|
||||||
UASSERT(filteredCoordinates.size() == textureMesh->tex_polygons[t].size()*polygonSize);
|
|
||||||
for(unsigned int i=0; i<textureMesh->tex_polygons[t].size(); ++i)
|
|
||||||
{
|
|
||||||
if(validPolygons.find(allPolygonsIndex) != validPolygons.end())
|
|
||||||
{
|
|
||||||
filteredPolygons[oi] = textureMesh->tex_polygons[t].at(i);
|
|
||||||
for(unsigned int j=0; j<polygonSize; ++j)
|
|
||||||
{
|
|
||||||
filteredCoordinates[oi*polygonSize + j] = textureMesh->tex_coordinates[t][i*polygonSize + j];
|
|
||||||
}
|
|
||||||
++oi;
|
|
||||||
}
|
|
||||||
++allPolygonsIndex;
|
|
||||||
}
|
|
||||||
filteredPolygons.resize(oi);
|
|
||||||
filteredCoordinates.resize(oi*polygonSize);
|
|
||||||
textureMesh->tex_polygons[t] = filteredPolygons;
|
|
||||||
textureMesh->tex_coordinates[t] = filteredCoordinates;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
LOGI("Filtered %d polygons.", (int)(allPolygons.size()-validPolygons.size()));
|
|
||||||
}
|
|
||||||
|
|
||||||
for(unsigned int t=0; t<textureMesh->tex_polygons.size(); ++t)
|
|
||||||
{
|
|
||||||
totalPolygons+=textureMesh->tex_polygons[t].size();
|
|
||||||
}
|
|
||||||
|
|
||||||
LOGI("Cleanup mesh... done! %fs (total polygons=%d)", timer.ticks(), totalPolygons);
|
|
||||||
}
|
}
|
||||||
else
|
|
||||||
|
totalPolygons = 0;
|
||||||
|
for(unsigned int t=0; t<textureMesh->tex_polygons.size(); ++t)
|
||||||
{
|
{
|
||||||
for(unsigned int t=0; t<textureMesh->tex_polygons.size(); ++t)
|
totalPolygons+=textureMesh->tex_polygons[t].size();
|
||||||
{
|
|
||||||
totalPolygons+=textureMesh->tex_polygons[t].size();
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
totalPolygons = (int)mesh->polygons.size();
|
||||||
|
polygonMesh = mesh;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1960,6 +1960,11 @@ public class RTABMapActivity extends Activity implements OnClickListener {
|
|||||||
{
|
{
|
||||||
mItemRenderingTextureMesh.setChecked(true);
|
mItemRenderingTextureMesh.setChecked(true);
|
||||||
}
|
}
|
||||||
|
if(!optimizedCleanWhitePolygons)
|
||||||
|
{
|
||||||
|
mButtonLighting.setChecked(true);
|
||||||
|
RTABMapLib.setLighting(true);
|
||||||
|
}
|
||||||
updateState(State.STATE_VISUALIZING);
|
updateState(State.STATE_VISUALIZING);
|
||||||
RTABMapLib.postExportation(true);
|
RTABMapLib.postExportation(true);
|
||||||
if(mButtonFirst.isChecked())
|
if(mButtonFirst.isChecked())
|
||||||
|
|||||||
@@ -48,7 +48,6 @@ std::vector<pcl::Vertices> normalizePolygonsSide(
|
|||||||
template<typename pointRGBT>
|
template<typename pointRGBT>
|
||||||
void denseMeshPostProcessing(
|
void denseMeshPostProcessing(
|
||||||
pcl::PolygonMeshPtr & mesh,
|
pcl::PolygonMeshPtr & mesh,
|
||||||
bool hasColors,
|
|
||||||
float meshDecimationFactor,
|
float meshDecimationFactor,
|
||||||
int maximumPolygons,
|
int maximumPolygons,
|
||||||
const typename pcl::PointCloud<pointRGBT>::Ptr & cloud,
|
const typename pcl::PointCloud<pointRGBT>::Ptr & cloud,
|
||||||
@@ -58,6 +57,21 @@ void denseMeshPostProcessing(
|
|||||||
int minClusterSize,
|
int minClusterSize,
|
||||||
ProgressState * progressState)
|
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)
|
if(maximumPolygons > 0)
|
||||||
{
|
{
|
||||||
double factor = 1.0-double(maximumPolygons)/double(mesh->polygons.size());
|
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!"));
|
if(progressState) progressState->callback(uFormat("Decimated mesh has more polygons than before!"));
|
||||||
}
|
}
|
||||||
|
hasNormals = false;
|
||||||
hasColors = false;
|
hasColors = false;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -90,7 +105,7 @@ void denseMeshPostProcessing(
|
|||||||
// transfer color from point cloud to mesh
|
// transfer color from point cloud to mesh
|
||||||
typename pcl::search::KdTree<pointRGBT>::Ptr tree (new pcl::search::KdTree<pointRGBT>(true));
|
typename pcl::search::KdTree<pointRGBT>::Ptr tree (new pcl::search::KdTree<pointRGBT>(true));
|
||||||
tree->setInputCloud(cloud);
|
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);
|
pcl::fromPCLPointCloud2(mesh->cloud, *coloredCloud);
|
||||||
std::vector<bool> coloredPts(coloredCloud->size());
|
std::vector<bool> coloredPts(coloredCloud->size());
|
||||||
for(unsigned int i=0; i<coloredCloud->size(); ++i)
|
for(unsigned int i=0; i<coloredCloud->size(); ++i)
|
||||||
@@ -162,6 +177,7 @@ void denseMeshPostProcessing(
|
|||||||
filteredPolygons.resize(oi);
|
filteredPolygons.resize(oi);
|
||||||
mesh->polygons = filteredPolygons;
|
mesh->polygons = filteredPolygons;
|
||||||
}
|
}
|
||||||
|
hasColors = true;
|
||||||
}
|
}
|
||||||
else if(cloud.get()!=0 &&
|
else if(cloud.get()!=0 &&
|
||||||
!hasColors &&
|
!hasColors &&
|
||||||
@@ -174,7 +190,7 @@ void denseMeshPostProcessing(
|
|||||||
// transfer color from point cloud to mesh
|
// transfer color from point cloud to mesh
|
||||||
typename pcl::search::KdTree<pointRGBT>::Ptr tree (new pcl::search::KdTree<pointRGBT>(true));
|
typename pcl::search::KdTree<pointRGBT>::Ptr tree (new pcl::search::KdTree<pointRGBT>(true));
|
||||||
tree->setInputCloud(cloud);
|
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);
|
pcl::fromPCLPointCloud2(mesh->cloud, *optimizedCloud);
|
||||||
std::vector<bool> closePts(optimizedCloud->size());
|
std::vector<bool> closePts(optimizedCloud->size());
|
||||||
for(unsigned int i=0; i<optimizedCloud->size(); ++i)
|
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()));
|
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);
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -274,7 +274,6 @@ std::vector<pcl::Vertices> normalizePolygonsSide(
|
|||||||
template<typename pointRGBT>
|
template<typename pointRGBT>
|
||||||
void denseMeshPostProcessing(
|
void denseMeshPostProcessing(
|
||||||
pcl::PolygonMeshPtr & mesh,
|
pcl::PolygonMeshPtr & mesh,
|
||||||
bool hasColors, // Tell if the mesh has colors
|
|
||||||
float meshDecimationFactor = 0.0f, // value between 0 and 1, 0=disabled
|
float meshDecimationFactor = 0.0f, // value between 0 and 1, 0=disabled
|
||||||
int maximumPolygons = 0, // 0=disabled
|
int maximumPolygons = 0, // 0=disabled
|
||||||
const typename pcl::PointCloud<pointRGBT>::Ptr & cloud = pcl::PointCloud<pointRGBT>::Ptr(), // A RGB point cloud used to transfer colors back to mesh (needed for parameters below)
|
const typename pcl::PointCloud<pointRGBT>::Ptr & cloud = pcl::PointCloud<pointRGBT>::Ptr(), // A RGB point cloud used to transfer colors back to mesh (needed for parameters below)
|
||||||
|
|||||||
@@ -1854,7 +1854,6 @@ bool ExportCloudsDialog::getExportedClouds(
|
|||||||
TexturingState texturingState(_progressDialog, false);
|
TexturingState texturingState(_progressDialog, false);
|
||||||
util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
|
util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
|
||||||
mesh,
|
mesh,
|
||||||
!lostColors,
|
|
||||||
_ui->doubleSpinBox_meshDecimationFactor->isEnabled()?(float)_ui->doubleSpinBox_meshDecimationFactor->value():0.0f,
|
_ui->doubleSpinBox_meshDecimationFactor->isEnabled()?(float)_ui->doubleSpinBox_meshDecimationFactor->value():0.0f,
|
||||||
_ui->spinBox_meshMaxPolygons->isEnabled()?_ui->spinBox_meshMaxPolygons->value():0,
|
_ui->spinBox_meshMaxPolygons->isEnabled()?_ui->spinBox_meshMaxPolygons->value():0,
|
||||||
iter->second,
|
iter->second,
|
||||||
@@ -1965,7 +1964,6 @@ bool ExportCloudsDialog::getExportedClouds(
|
|||||||
TexturingState texturingState(_progressDialog, false);
|
TexturingState texturingState(_progressDialog, false);
|
||||||
util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
|
util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
|
||||||
mesh,
|
mesh,
|
||||||
true,
|
|
||||||
_ui->doubleSpinBox_meshDecimationFactor->isEnabled()?(float)_ui->doubleSpinBox_meshDecimationFactor->value():0.0f,
|
_ui->doubleSpinBox_meshDecimationFactor->isEnabled()?(float)_ui->doubleSpinBox_meshDecimationFactor->value():0.0f,
|
||||||
_ui->spinBox_meshMaxPolygons->isEnabled()?_ui->spinBox_meshMaxPolygons->value():0,
|
_ui->spinBox_meshMaxPolygons->isEnabled()?_ui->spinBox_meshMaxPolygons->value():0,
|
||||||
vertices,
|
vertices,
|
||||||
|
|||||||
Reference in New Issue
Block a user