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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:38:58