util3d_surface.hpp
Go to the documentation of this file.
1 /*
2  * util3d_surface.hpp
3  *
4  * Created on: Sep 3, 2016
5  * Author: mathieu
6  */
7 
8 #ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_
9 #define CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_
10 
11 #include <pcl/search/kdtree.h>
12 #include <rtabmap/utilite/UConversion.h>
13 
14 namespace rtabmap {
15 
16 namespace util3d {
17 
18 template<typename pointT>
19 std::vector<pcl::Vertices> normalizePolygonsSide(
20  const typename pcl::PointCloud<pointT> & cloud,
21  const std::vector<pcl::Vertices> & polygons,
22  const pcl::PointXYZ & viewPoint)
23 {
24  std::vector<pcl::Vertices> output(polygons.size());
25  for(unsigned int i=0; i<polygons.size(); ++i)
26  {
27  pcl::Vertices polygon = polygons[i];
28  Eigen::Vector3f v1 = cloud.at(polygon.vertices[1]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
29  Eigen::Vector3f v2 = cloud.at(polygon.vertices[2]).getVector3fMap() - cloud.at(polygon.vertices[0]).getVector3fMap();
30  Eigen::Vector3f n = (v1.cross(v2)).normalized();
31 
32  Eigen::Vector3f p = Eigen::Vector3f(viewPoint.x, viewPoint.y, viewPoint.z) - cloud.at(polygon.vertices[1]).getVector3fMap();
33 
34  float result = n.dot(p);
35  if(result < 0)
36  {
37  //reverse vertices order
38  int tmp = polygon.vertices[0];
39  polygon.vertices[0] = polygon.vertices[2];
40  polygon.vertices[2] = tmp;
41  }
42 
43  output[i] = polygon;
44  }
45  return output;
46 }
47 
48 template<typename pointRGBT>
49 void denseMeshPostProcessing(
50  pcl::PolygonMeshPtr & mesh,
51  float meshDecimationFactor,
52  int maximumPolygons,
53  const typename pcl::PointCloud<pointRGBT>::Ptr & cloud,
54  float transferColorRadius,
55  bool coloredOutput,
56  bool cleanMesh,
57  int minClusterSize,
58  ProgressState * progressState)
59 {
60  // compute normals for the mesh if not already here
61  bool hasNormals = false;
62  bool hasColors = false;
63  for(unsigned int i=0; i<mesh->cloud.fields.size(); ++i)
64  {
65  if(mesh->cloud.fields[i].name.compare("normal_x") == 0)
66  {
67  hasNormals = true;
68  }
69  else if(mesh->cloud.fields[i].name.compare("rgb") == 0)
70  {
71  hasColors = true;
72  }
73  }
74 
75  if(maximumPolygons > 0)
76  {
77  double factor = 1.0-double(maximumPolygons)/double(mesh->polygons.size());
78  if(factor > meshDecimationFactor)
79  {
80  meshDecimationFactor = factor;
81  }
82  }
83  if(meshDecimationFactor > 0.0)
84  {
85  unsigned int count = mesh->polygons.size();
86  if(progressState) progressState->callback(uFormat("Mesh decimation (factor=%f) from %d polygons...",meshDecimationFactor, (int)count));
87 
88  mesh = util3d::meshDecimation(mesh, (float)meshDecimationFactor);
89  if(progressState) progressState->callback(uFormat("Mesh decimated (factor=%f) from %d to %d polygons", meshDecimationFactor, (int)count, (int)mesh->polygons.size()));
90  if(count < mesh->polygons.size())
91  {
92  if(progressState) progressState->callback(uFormat("Decimated mesh has more polygons than before!"));
93  }
94  hasNormals = false;
95  hasColors = false;
96  }
97 
98  if(cloud.get()!=0 &&
99  !hasColors &&
100  transferColorRadius >= 0.0)
101  {
102  if(progressState) progressState->callback(uFormat("Transferring color from point cloud to mesh..."));
103 
104  // transfer color from point cloud to mesh
105  typename pcl::search::KdTree<pointRGBT>::Ptr tree (new pcl::search::KdTree<pointRGBT>(true));
106  tree->setInputCloud(cloud);
107  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr coloredCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
108  pcl::fromPCLPointCloud2(mesh->cloud, *coloredCloud);
109  std::vector<bool> coloredPts(coloredCloud->size());
110  for(unsigned int i=0; i<coloredCloud->size(); ++i)
111  {
112  std::vector<int> kIndices;
113  std::vector<float> kDistances;
114  pointRGBT pt;
115  pt.x = coloredCloud->at(i).x;
116  pt.y = coloredCloud->at(i).y;
117  pt.z = coloredCloud->at(i).z;
118  if(transferColorRadius > 0.0)
119  {
120  tree->radiusSearch(pt, transferColorRadius, kIndices, kDistances);
121  }
122  else
123  {
124  tree->nearestKSearch(pt, 1, kIndices, kDistances);
125  }
126  if(kIndices.size())
127  {
128  //compute average color
129  int r=0;
130  int g=0;
131  int b=0;
132  int a=0;
133  for(unsigned int j=0; j<kIndices.size(); ++j)
134  {
135  r+=(int)cloud->at(kIndices[j]).r;
136  g+=(int)cloud->at(kIndices[j]).g;
137  b+=(int)cloud->at(kIndices[j]).b;
138  a+=(int)cloud->at(kIndices[j]).a;
139  }
140  coloredCloud->at(i).r = r/kIndices.size();
141  coloredCloud->at(i).g = g/kIndices.size();
142  coloredCloud->at(i).b = b/kIndices.size();
143  coloredCloud->at(i).a = a/kIndices.size();
144  coloredPts.at(i) = true;
145  }
146  else
147  {
148  //white
149  coloredCloud->at(i).r = coloredCloud->at(i).g = coloredCloud->at(i).b = 255;
150  coloredPts.at(i) = false;
151  }
152  }
153  pcl::toPCLPointCloud2(*coloredCloud, mesh->cloud);
154 
155  // remove polygons with no color
156  if(cleanMesh)
157  {
158  std::vector<pcl::Vertices> filteredPolygons(mesh->polygons.size());
159  int oi=0;
160  for(unsigned int i=0; i<mesh->polygons.size(); ++i)
161  {
162  bool coloredPolygon = true;
163  for(unsigned int j=0; j<mesh->polygons[i].vertices.size(); ++j)
164  {
165  if(!coloredPts.at(mesh->polygons[i].vertices[j]))
166  {
167  coloredPolygon = false;
168  break;
169  }
170  }
171  if(coloredPolygon)
172  {
173  filteredPolygons[oi++] = mesh->polygons[i];
174  }
175  }
176  filteredPolygons.resize(oi);
177  mesh->polygons = filteredPolygons;
178  }
179  hasColors = true;
180  }
181 
182  if(minClusterSize)
183  {
184  if(progressState) progressState->callback(uFormat("Filter small polygon clusters..."));
185 
186  // filter polygons
187  std::vector<std::set<int> > neighbors;
188  std::vector<std::set<int> > vertexToPolygons;
189  util3d::createPolygonIndexes(mesh->polygons,
190  mesh->cloud.height*mesh->cloud.width,
191  neighbors,
192  vertexToPolygons);
193  std::list<std::list<int> > clusters = util3d::clusterPolygons(
194  neighbors,
195  minClusterSize<0?0:minClusterSize);
196 
197  std::vector<pcl::Vertices> filteredPolygons(mesh->polygons.size());
198  if(minClusterSize < 0)
199  {
200  // only keep the biggest cluster
201  std::list<std::list<int> >::iterator biggestClusterIndex = clusters.end();
202  unsigned int biggestClusterSize = 0;
203  for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
204  {
205  if(iter->size() > biggestClusterSize)
206  {
207  biggestClusterIndex = iter;
208  biggestClusterSize = iter->size();
209  }
210  }
211  if(biggestClusterIndex != clusters.end())
212  {
213  int oi=0;
214  for(std::list<int>::iterator jter=biggestClusterIndex->begin(); jter!=biggestClusterIndex->end(); ++jter)
215  {
216  filteredPolygons[oi++] = mesh->polygons.at(*jter);
217  }
218  filteredPolygons.resize(oi);
219  }
220  }
221  else
222  {
223  int oi=0;
224  for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
225  {
226  for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
227  {
228  filteredPolygons[oi++] = mesh->polygons.at(*jter);
229  }
230  }
231  filteredPolygons.resize(oi);
232  }
233 
234  int before = (int)mesh->polygons.size();
235  mesh->polygons = filteredPolygons;
236 
237  if(progressState) progressState->callback(uFormat("Filtered %d polygons.", before-(int)mesh->polygons.size()));
238  }
239 
240  // compute normals for the mesh if not already here, add also white color if colored output is required
241  if(!hasNormals || (!hasColors && coloredOutput))
242  {
243  // use polygons
244  if(hasColors || coloredOutput)
245  {
246  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
247  pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
248 
249  Eigen::Vector3f normal(1,0,0);
250  for(unsigned int i=0; i<mesh->polygons.size(); ++i)
251  {
252  pcl::Vertices & v = mesh->polygons[i];
253  if(!hasNormals)
254  {
255  UASSERT(v.vertices.size()>2);
256  Eigen::Vector3f v0(
257  cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
258  cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
259  cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
260  int last = v.vertices.size()-1;
261  Eigen::Vector3f v1(
262  cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
263  cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
264  cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
265  normal = v0.cross(v1);
266  normal.normalize();
267  }
268  // flat normal (per face)
269  for(unsigned int j=0; j<v.vertices.size(); ++j)
270  {
271  if(!hasNormals)
272  {
273  cloud->at(v.vertices[j]).normal_x = normal[0];
274  cloud->at(v.vertices[j]).normal_y = normal[1];
275  cloud->at(v.vertices[j]).normal_z = normal[2];
276  }
277  if(!hasColors)
278  {
279  cloud->at(v.vertices[j]).r = 255;
280  cloud->at(v.vertices[j]).g = 255;
281  cloud->at(v.vertices[j]).b = 255;
282  }
283  }
284  }
285  pcl::toPCLPointCloud2 (*cloud, mesh->cloud);
286  }
287  else
288  {
289  pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
290  pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
291 
292  for(unsigned int i=0; i<mesh->polygons.size(); ++i)
293  {
294  pcl::Vertices & v = mesh->polygons[i];
295  UASSERT(v.vertices.size()>2);
296  Eigen::Vector3f v0(
297  cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
298  cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
299  cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
300  int last = v.vertices.size()-1;
301  Eigen::Vector3f v1(
302  cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
303  cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
304  cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
305  Eigen::Vector3f normal = v0.cross(v1);
306  normal.normalize();
307  // flat normal (per face)
308  for(unsigned int j=0; j<v.vertices.size(); ++j)
309  {
310  cloud->at(v.vertices[j]).normal_x = normal[0];
311  cloud->at(v.vertices[j]).normal_y = normal[1];
312  cloud->at(v.vertices[j]).normal_z = normal[2];
313  }
314  }
315  pcl::toPCLPointCloud2 (*cloud, mesh->cloud);
316  }
317  }
318 }
319 
320 }
321 
322 }
323 
324 
325 #endif /* CORELIB_INCLUDE_RTABMAP_CORE_IMPL_UTIL3D_SURFACE_HPP_ */
rtabmap::util3d::denseMeshPostProcessing
void denseMeshPostProcessing(pcl::PolygonMeshPtr &mesh, float meshDecimationFactor, int maximumPolygons, const typename pcl::PointCloud< pointRGBT >::Ptr &cloud, float transferColorRadius, bool coloredOutput, bool cleanMesh, int minClusterSize, ProgressState *progressState)
Definition: util3d_surface.hpp:49
UConversion.h
Some conversion functions.
UASSERT
#define UASSERT(condition)
rtabmap::ProgressState::callback
virtual bool callback(const std::string &msg) const
Definition: ProgressState.h:39
rtabmap::util3d::clusterPolygons
std::list< std::list< int > > RTABMAP_EXP clusterPolygons(const std::vector< std::set< int > > &neighborPolygons, int minClusterSize=0)
Definition: util3d_surface.cpp:116
rtabmap::util3d::meshDecimation
pcl::PolygonMesh::Ptr RTABMAP_EXP meshDecimation(const pcl::PolygonMesh::Ptr &mesh, float factor)
Definition: util3d_surface.cpp:3008
tree
tree
rtabmap::util3d::normalizePolygonsSide
std::vector< pcl::Vertices > normalizePolygonsSide(const typename pcl::PointCloud< pointT > &cloud, const std::vector< pcl::Vertices > &polygons, const pcl::PointXYZ &viewPoint)
Definition: util3d_surface.hpp:19
rtabmap::util3d::createPolygonIndexes
void RTABMAP_EXP createPolygonIndexes(const std::vector< pcl::Vertices > &polygons, int cloudSize, std::vector< std::set< int > > &neighborPolygons, std::vector< std::set< int > > &vertexPolygons)
Given a set of polygons, create two indexes: polygons to neighbor polygons and vertices to polygons...
Definition: util3d_surface.cpp:79
tango_gl::kIndices
static const GLushort kIndices[]
Definition: video_overlay.cpp:28
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:43:41