diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 0dfddd44..720752fc 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -2121,9 +2121,11 @@ bool RTABMapApp::exportMesh( 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(); float factor = 1.0f-float(optimizedMaxPolygons)/float(count); 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!"); } - #else +#else UWARN("RTAB-Map is not built with PCL-VTK module so mesh decimation cannot be used!"); - #endif +#endif } if(progressionStatus_.isCanceled()) @@ -2164,206 +2166,18 @@ bool RTABMapApp::exportMesh( progressionStatus_.increment(); - if(textureSize == 0) + rtabmap::util3d::denseMeshPostProcessing( + 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::Ptr tree (new pcl::search::KdTree(true)); - tree->setInputCloud(mergedClouds); - pcl::PointCloud::Ptr coloredCloud(new pcl::PointCloud); - pcl::fromPCLPointCloud2(mesh->cloud, *coloredCloud); - std::vector coloredPts(coloredCloud->size()); - for(unsigned int i=0; isize(); ++i) - { - std::vector kIndices; - std::vector 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; jat(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 filteredPolygons(optimizedCleanWhitePolygons?mesh->polygons.size():0); - int oi=0; - for(unsigned int i=0; ipolygons.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; jat(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; jpolygons[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::Ptr cloud (new pcl::PointCloud); - pcl::fromPCLPointCloud2(mesh->cloud, *cloud); - - for(unsigned int i=0; ipolygons.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; jat(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::Ptr tree (new pcl::search::KdTree(true)); - tree->setInputCloud(mergedClouds); - pcl::PointCloud::Ptr optimizedCloud(new pcl::PointCloud); - pcl::fromPCLPointCloud2(mesh->cloud, *optimizedCloud); - std::vector closePts(optimizedCloud->size()); - for(unsigned int i=0; isize(); ++i) - { - std::vector kIndices; - std::vector 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 filteredPolygons(mesh->polygons.size()); - int oi=0; - for(unsigned int i=0; ipolygons.size(); ++i) - { - bool keepPolygon = true; - for(unsigned int j=0; jpolygons[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..."); textureMesh = rtabmap::util3d::createTextureMesh( mesh, @@ -2393,109 +2207,21 @@ bool RTABMapApp::exportMesh( if(textureMesh->tex_coordinates.size() && optimizedCleanWhitePolygons) { LOGI("Cleanup mesh..."); - - // assume last texture is the occluded texture - 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; ttex_polygons.size(); ++t) - { - totalSize+=textureMesh->tex_polygons[t].size(); - } - std::vector allPolygons(totalSize); - int oi=0; - for(unsigned int t=0; ttex_polygons.size(); ++t) - { - for(unsigned int i=0; itex_polygons[t].size(); ++i) - { - allPolygons[oi++] = textureMesh->tex_polygons[t][i]; - } - } - - // filter polygons - std::vector > neighbors; - std::vector > vertexToPolygons; - rtabmap::util3d::createPolygonIndexes(allPolygons, - textureMesh->cloud.data.size()/textureMesh->cloud.point_step, - neighbors, - vertexToPolygons); - std::list > clusters = rtabmap::util3d::clusterPolygons( - neighbors, - optimizedMinTextureClusterSize); - - std::set validPolygons; - for(std::list >::iterator kter=clusters.begin(); kter!=clusters.end(); ++kter) - { - for(std::list::iterator jter=kter->begin(); jter!=kter->end(); ++jter) - { - validPolygons.insert(*jter); - } - } - - // for each texture - unsigned int allPolygonsIndex = 0; - for(unsigned int t=0; ttex_polygons.size(); ++t) - { - std::vector filteredPolygons(textureMesh->tex_polygons[t].size()); - #if PCL_VERSION_COMPARE(>=, 1, 8, 0) - std::vector > filteredCoordinates(textureMesh->tex_coordinates[t].size()); - #else - std::vector 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; itex_polygons[t].size(); ++i) - { - if(validPolygons.find(allPolygonsIndex) != validPolygons.end()) - { - filteredPolygons[oi] = textureMesh->tex_polygons[t].at(i); - for(unsigned int j=0; jtex_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; ttex_polygons.size(); ++t) - { - totalPolygons+=textureMesh->tex_polygons[t].size(); - } - - LOGI("Cleanup mesh... done! %fs (total polygons=%d)", timer.ticks(), totalPolygons); + rtabmap::util3d::cleanTextureMesh(*textureMesh, 0); + LOGI("Cleanup mesh... done! %fs", timer.ticks()); } - else + + totalPolygons = 0; + for(unsigned int t=0; ttex_polygons.size(); ++t) { - for(unsigned int t=0; ttex_polygons.size(); ++t) - { - totalPolygons+=textureMesh->tex_polygons[t].size(); - } + totalPolygons+=textureMesh->tex_polygons[t].size(); } } + else + { + totalPolygons = (int)mesh->polygons.size(); + polygonMesh = mesh; + } } } } diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java index 03481284..59ae37f4 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java @@ -1960,6 +1960,11 @@ public class RTABMapActivity extends Activity implements OnClickListener { { mItemRenderingTextureMesh.setChecked(true); } + if(!optimizedCleanWhitePolygons) + { + mButtonLighting.setChecked(true); + RTABMapLib.setLighting(true); + } updateState(State.STATE_VISUALIZING); RTABMapLib.postExportation(true); if(mButtonFirst.isChecked()) diff --git a/corelib/include/rtabmap/core/impl/util3d_surface.hpp b/corelib/include/rtabmap/core/impl/util3d_surface.hpp index 7909a7b6..c8ee1b1a 100644 --- a/corelib/include/rtabmap/core/impl/util3d_surface.hpp +++ b/corelib/include/rtabmap/core/impl/util3d_surface.hpp @@ -48,7 +48,6 @@ std::vector normalizePolygonsSide( template void denseMeshPostProcessing( pcl::PolygonMeshPtr & mesh, - bool hasColors, float meshDecimationFactor, int maximumPolygons, const typename pcl::PointCloud::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; icloud.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::Ptr tree (new pcl::search::KdTree(true)); tree->setInputCloud(cloud); - pcl::PointCloud::Ptr coloredCloud(new pcl::PointCloud); + pcl::PointCloud::Ptr coloredCloud(new pcl::PointCloud); pcl::fromPCLPointCloud2(mesh->cloud, *coloredCloud); std::vector coloredPts(coloredCloud->size()); for(unsigned int i=0; isize(); ++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::Ptr tree (new pcl::search::KdTree(true)); tree->setInputCloud(cloud); - pcl::PointCloud::Ptr optimizedCloud(new pcl::PointCloud); + pcl::PointCloud::Ptr optimizedCloud(new pcl::PointCloud); pcl::fromPCLPointCloud2(mesh->cloud, *optimizedCloud); std::vector closePts(optimizedCloud->size()); for(unsigned int i=0; isize(); ++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::Ptr cloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2(mesh->cloud, *cloud); + + Eigen::Vector3f normal(1,0,0); + for(unsigned int i=0; ipolygons.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; jat(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::Ptr cloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2(mesh->cloud, *cloud); + + for(unsigned int i=0; ipolygons.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; jat(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); + } + } } } diff --git a/corelib/include/rtabmap/core/util3d_surface.h b/corelib/include/rtabmap/core/util3d_surface.h index a2fc915b..6d9c7c01 100644 --- a/corelib/include/rtabmap/core/util3d_surface.h +++ b/corelib/include/rtabmap/core/util3d_surface.h @@ -274,7 +274,6 @@ std::vector normalizePolygonsSide( template void denseMeshPostProcessing( pcl::PolygonMeshPtr & mesh, - bool hasColors, // Tell if the mesh has colors float meshDecimationFactor = 0.0f, // value between 0 and 1, 0=disabled int maximumPolygons = 0, // 0=disabled const typename pcl::PointCloud::Ptr & cloud = pcl::PointCloud::Ptr(), // A RGB point cloud used to transfer colors back to mesh (needed for parameters below) diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index 90d30fda..0d9c0d7a 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -1854,7 +1854,6 @@ bool ExportCloudsDialog::getExportedClouds( TexturingState texturingState(_progressDialog, false); util3d::denseMeshPostProcessing( mesh, - !lostColors, _ui->doubleSpinBox_meshDecimationFactor->isEnabled()?(float)_ui->doubleSpinBox_meshDecimationFactor->value():0.0f, _ui->spinBox_meshMaxPolygons->isEnabled()?_ui->spinBox_meshMaxPolygons->value():0, iter->second, @@ -1965,7 +1964,6 @@ bool ExportCloudsDialog::getExportedClouds( TexturingState texturingState(_progressDialog, false); util3d::denseMeshPostProcessing( mesh, - true, _ui->doubleSpinBox_meshDecimationFactor->isEnabled()?(float)_ui->doubleSpinBox_meshDecimationFactor->value():0.0f, _ui->spinBox_meshMaxPolygons->isEnabled()?_ui->spinBox_meshMaxPolygons->value():0, vertices,