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_ */
int
int
rtabmap::util3d::createPolygonIndexes
void RTABMAP_CORE_EXPORT 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
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
v1
v1
b
Array< int, 3, 1 > b
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
tree
count
Index count
rtabmap::util3d::clusterPolygons
std::list< std::list< int > > RTABMAP_CORE_EXPORT clusterPolygons(const std::vector< std::set< int > > &neighborPolygons, int minClusterSize=0)
Definition: util3d_surface.cpp:131
rtabmap::ProgressState::callback
virtual bool callback(const std::string &msg) const
Definition: ProgressState.h:39
iterator
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
last
static const symbolic::SymbolExpr< internal::symbolic_last_tag > last
rtabmap::util3d::intersectRayTriangle
bool RTABMAP_CORE_EXPORT 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:3776
n
int n
g
float g
j
std::ptrdiff_t j
UConversion.h
Some conversion functions.
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
UASSERT
#define UASSERT(condition)
d
d
rtabmap::ProgressState
Definition: ProgressState.h:35
p
Point3_ p(2)
rtabmap::util3d::meshDecimation
pcl::PolygonMesh::Ptr RTABMAP_CORE_EXPORT meshDecimation(const pcl::PolygonMesh::Ptr &mesh, float factor)
Definition: util3d_surface.cpp:3761
normal
Unit3_ normal(const OrientedPlane3_ &p)
tango_gl::kIndices
static const GLushort kIndices[]
Definition: video_overlay.cpp:28
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
a
ArrayXXi a
iter
iterator iter(handle obj)
origin
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set origin
v
Array< int, Dynamic, 1 > v
distance
Double_ distance(const OrientedPlane3_ &p)
rtabmap
Definition: CameraARCore.cpp:35
i
int i
result
RESULT & result
v2
v2

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:24