rtabmap: util3d_surface.hpp Source File

Go to the documentation of this file.
00001 /*
00002  * util3d_surface.hpp
00003  *
00004  *  Created on: Sep 3, 2016
00005  *      Author: mathieu
00006  */
00007 
00008 #ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_
00009 #define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_
00010 
00011 #include <pcl/search/kdtree.h>
00012 #include <rtabmap/utilite/UConversion.h>
00013 
00014 namespace rtabmap {
00015 
00016 namespace util3d {
00017 
00018 template<typename pointT>
00019 std::vector<pcl::Vertices> normalizePolygonsSide(
00020                 const typename pcl::PointCloud<pointT> & cloud,
00021                 const std::vector<pcl::Vertices> & polygons,
00022                 const pcl::PointXYZ & viewPoint)
00023 {
00024         std::vector<pcl::Vertices> output(polygons.size());
00025         for(unsigned int i=0; i<polygons.size(); ++i)
00026         {
00027                 pcl::Vertices polygon = polygons[i];
00028                 Eigen::Vector3f v1 = cloud.at(polygon.vertices[1]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
00029                 Eigen::Vector3f v2 = cloud.at(polygon.vertices[2]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
00030                 Eigen::Vector3f n = (v1.cross(v2)).normalized();
00031 
00032                 Eigen::Vector3f p = Eigen::Vector3f(viewPoint.x, viewPoint.y, viewPoint.z) - cloud.at(polygon.vertices[1]).getVector3fMap();
00033 
00034                 float result = n.dot(p);
00035                 if(result < 0)
00036                 {
00037                         //reverse vertices order
00038                         int tmp = polygon.vertices[0];
00039                         polygon.vertices[0] = polygon.vertices[2];
00040                         polygon.vertices[2] = tmp;
00041                 }
00042 
00043                 output[i] = polygon;
00044         }
00045         return output;
00046 }
00047 
00048 template<typename pointRGBT>
00049 void denseMeshPostProcessing(
00050                 pcl::PolygonMeshPtr & mesh,
00051                 float meshDecimationFactor,
00052                 int maximumPolygons,
00053                 const typename pcl::PointCloud<pointRGBT>::Ptr & cloud,
00054                 float transferColorRadius,
00055                 bool coloredOutput,
00056                 bool cleanMesh,
00057                 int minClusterSize,
00058                 ProgressState * progressState)
00059 {
00060         // compute normals for the mesh if not already here
00061         bool hasNormals = false;
00062         bool hasColors = false;
00063         for(unsigned int i=0; i<mesh->cloud.fields.size(); ++i)
00064         {
00065                 if(mesh->cloud.fields[i].name.compare("normal_x") == 0)
00066                 {
00067                         hasNormals = true;
00068                 }
00069                 else if(mesh->cloud.fields[i].name.compare("rgb") == 0)
00070                 {
00071                         hasColors = true;
00072                 }
00073         }
00074 
00075         if(maximumPolygons > 0)
00076         {
00077                 double factor = 1.0-double(maximumPolygons)/double(mesh->polygons.size());
00078                 if(factor > meshDecimationFactor)
00079                 {
00080                         meshDecimationFactor = factor;
00081                 }
00082         }
00083         if(meshDecimationFactor > 0.0)
00084         {
00085                 unsigned int count = mesh->polygons.size();
00086                 if(progressState) progressState->callback(uFormat("Mesh decimation (factor=%f) from %d polygons...",meshDecimationFactor, (int)count));
00087 
00088                 mesh = util3d::meshDecimation(mesh, (float)meshDecimationFactor);
00089                 if(progressState) progressState->callback(uFormat("Mesh decimated (factor=%f) from %d to %d polygons", meshDecimationFactor, (int)count, (int)mesh->polygons.size()));
00090                 if(count < mesh->polygons.size())
00091                 {
00092                         if(progressState) progressState->callback(uFormat("Decimated mesh has more polygons than before!"));
00093                 }
00094                 hasNormals = false;
00095                 hasColors = false;
00096         }
00097 
00098         if(cloud.get()!=0 &&
00099                 !hasColors &&
00100                 transferColorRadius >= 0.0)
00101         {
00102                 if(progressState) progressState->callback(uFormat("Transferring color from point cloud to mesh..."));
00103 
00104                 // transfer color from point cloud to mesh
00105                 typename pcl::search::KdTree<pointRGBT>::Ptr tree (new pcl::search::KdTree<pointRGBT>(true));
00106                 tree->setInputCloud(cloud);
00107                 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr coloredCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00108                 pcl::fromPCLPointCloud2(mesh->cloud, *coloredCloud);
00109                 std::vector<bool> coloredPts(coloredCloud->size());
00110                 for(unsigned int i=0; i<coloredCloud->size(); ++i)
00111                 {
00112                         std::vector<int> kIndices;
00113                         std::vector<float> kDistances;
00114                         pointRGBT pt;
00115                         pt.x = coloredCloud->at(i).x;
00116                         pt.y = coloredCloud->at(i).y;
00117                         pt.z = coloredCloud->at(i).z;
00118                         if(transferColorRadius > 0.0)
00119                         {
00120                                 tree->radiusSearch(pt, transferColorRadius, kIndices, kDistances);
00121                         }
00122                         else
00123                         {
00124                                 tree->nearestKSearch(pt, 1, kIndices, kDistances);
00125                         }
00126                         if(kIndices.size())
00127                         {
00128                                 //compute average color
00129                                 int r=0;
00130                                 int g=0;
00131                                 int b=0;
00132                                 int a=0;
00133                                 for(unsigned int j=0; j<kIndices.size(); ++j)
00134                                 {
00135                                         r+=(int)cloud->at(kIndices[j]).r;
00136                                         g+=(int)cloud->at(kIndices[j]).g;
00137                                         b+=(int)cloud->at(kIndices[j]).b;
00138                                         a+=(int)cloud->at(kIndices[j]).a;
00139                                 }
00140                                 coloredCloud->at(i).r = r/kIndices.size();
00141                                 coloredCloud->at(i).g = g/kIndices.size();
00142                                 coloredCloud->at(i).b = b/kIndices.size();
00143                                 coloredCloud->at(i).a = a/kIndices.size();
00144                                 coloredPts.at(i) = true;
00145                         }
00146                         else
00147                         {
00148                                 //white
00149                                 coloredCloud->at(i).r = coloredCloud->at(i).g = coloredCloud->at(i).b = 255;
00150                                 coloredPts.at(i) = false;
00151                         }
00152                 }
00153                 pcl::toPCLPointCloud2(*coloredCloud, mesh->cloud);
00154 
00155                 // remove polygons with no color
00156                 if(cleanMesh)
00157                 {
00158                         std::vector<pcl::Vertices> filteredPolygons(mesh->polygons.size());
00159                         int oi=0;
00160                         for(unsigned int i=0; i<mesh->polygons.size(); ++i)
00161                         {
00162                                 bool coloredPolygon = true;
00163                                 for(unsigned int j=0; j<mesh->polygons[i].vertices.size(); ++j)
00164                                 {
00165                                         if(!coloredPts.at(mesh->polygons[i].vertices[j]))
00166                                         {
00167                                                 coloredPolygon = false;
00168                                                 break;
00169                                         }
00170                                 }
00171                                 if(coloredPolygon)
00172                                 {
00173                                         filteredPolygons[oi++] = mesh->polygons[i];
00174                                 }
00175                         }
00176                         filteredPolygons.resize(oi);
00177                         mesh->polygons = filteredPolygons;
00178                 }
00179                 hasColors = true;
00180         }
00181 
00182         if(minClusterSize)
00183         {
00184                 if(progressState) progressState->callback(uFormat("Filter small polygon clusters..."));
00185 
00186                 // filter polygons
00187                 std::vector<std::set<int> > neighbors;
00188                 std::vector<std::set<int> > vertexToPolygons;
00189                 util3d::createPolygonIndexes(mesh->polygons,
00190                                 mesh->cloud.height*mesh->cloud.width,
00191                                 neighbors,
00192                                 vertexToPolygons);
00193                 std::list<std::list<int> > clusters = util3d::clusterPolygons(
00194                                 neighbors,
00195                                 minClusterSize<0?0:minClusterSize);
00196 
00197                 std::vector<pcl::Vertices> filteredPolygons(mesh->polygons.size());
00198                 if(minClusterSize < 0)
00199                 {
00200                         // only keep the biggest cluster
00201                         std::list<std::list<int> >::iterator biggestClusterIndex = clusters.end();
00202                         unsigned int biggestClusterSize = 0;
00203                         for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
00204                         {
00205                                 if(iter->size() > biggestClusterSize)
00206                                 {
00207                                         biggestClusterIndex = iter;
00208                                         biggestClusterSize = iter->size();
00209                                 }
00210                         }
00211                         if(biggestClusterIndex != clusters.end())
00212                         {
00213                                 int oi=0;
00214                                 for(std::list<int>::iterator jter=biggestClusterIndex->begin(); jter!=biggestClusterIndex->end(); ++jter)
00215                                 {
00216                                         filteredPolygons[oi++] = mesh->polygons.at(*jter);
00217                                 }
00218                                 filteredPolygons.resize(oi);
00219                         }
00220                 }
00221                 else
00222                 {
00223                         int oi=0;
00224                         for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
00225                         {
00226                                 for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
00227                                 {
00228                                         filteredPolygons[oi++] = mesh->polygons.at(*jter);
00229                                 }
00230                         }
00231                         filteredPolygons.resize(oi);
00232                 }
00233 
00234                 int before = (int)mesh->polygons.size();
00235                 mesh->polygons = filteredPolygons;
00236 
00237                 if(progressState) progressState->callback(uFormat("Filtered %d polygons.", before-(int)mesh->polygons.size()));
00238         }
00239 
00240         // compute normals for the mesh if not already here, add also white color if colored output is required
00241         if(!hasNormals || (!hasColors && coloredOutput))
00242         {
00243                 // use polygons
00244                 if(hasColors || coloredOutput)
00245                 {
00246                         pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00247                         pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
00248 
00249                         Eigen::Vector3f normal(1,0,0);
00250                         for(unsigned int i=0; i<mesh->polygons.size(); ++i)
00251                         {
00252                                 pcl::Vertices & v = mesh->polygons[i];
00253                                 if(!hasNormals)
00254                                 {
00255                                         UASSERT(v.vertices.size()>2);
00256                                         Eigen::Vector3f v0(
00257                                                         cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
00258                                                         cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
00259                                                         cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
00260                                         int last = v.vertices.size()-1;
00261                                         Eigen::Vector3f v1(
00262                                                         cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
00263                                                         cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
00264                                                         cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
00265                                         normal = v0.cross(v1);
00266                                         normal.normalize();
00267                                 }
00268                                 // flat normal (per face)
00269                                 for(unsigned int j=0; j<v.vertices.size(); ++j)
00270                                 {
00271                                         if(!hasNormals)
00272                                         {
00273                                                 cloud->at(v.vertices[j]).normal_x = normal[0];
00274                                                 cloud->at(v.vertices[j]).normal_y = normal[1];
00275                                                 cloud->at(v.vertices[j]).normal_z = normal[2];
00276                                         }
00277                                         if(!hasColors)
00278                                         {
00279                                                 cloud->at(v.vertices[j]).r = 255;
00280                                                 cloud->at(v.vertices[j]).g = 255;
00281                                                 cloud->at(v.vertices[j]).b = 255;
00282                                         }
00283                                 }
00284                         }
00285                         pcl::toPCLPointCloud2 (*cloud, mesh->cloud);
00286                 }
00287                 else
00288                 {
00289                         pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
00290                         pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
00291 
00292                         for(unsigned int i=0; i<mesh->polygons.size(); ++i)
00293                         {
00294                                 pcl::Vertices & v = mesh->polygons[i];
00295                                 UASSERT(v.vertices.size()>2);
00296                                 Eigen::Vector3f v0(
00297                                                 cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
00298                                                 cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
00299                                                 cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
00300                                 int last = v.vertices.size()-1;
00301                                 Eigen::Vector3f v1(
00302                                                 cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
00303                                                 cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
00304                                                 cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
00305                                 Eigen::Vector3f normal = v0.cross(v1);
00306                                 normal.normalize();
00307                                 // flat normal (per face)
00308                                 for(unsigned int j=0; j<v.vertices.size(); ++j)
00309                                 {
00310                                         cloud->at(v.vertices[j]).normal_x = normal[0];
00311                                         cloud->at(v.vertices[j]).normal_y = normal[1];
00312                                         cloud->at(v.vertices[j]).normal_z = normal[2];
00313                                 }
00314                         }
00315                         pcl::toPCLPointCloud2 (*cloud, mesh->cloud);
00316                 }
00317         }
00318 }
00319 
00320 }
00321 
00322 }
00323 
00324 
00325 #endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_ */