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  if(coloredOutput) {
155  pcl::toPCLPointCloud2(*coloredCloud, mesh->cloud);
156  hasColors = true;
157  }
158 
159  // remove polygons with no color
160  if(cleanMesh)
161  {
162  std::vector<pcl::Vertices> filteredPolygons(mesh->polygons.size());
163  int oi=0;
164  for(unsigned int i=0; i<mesh->polygons.size(); ++i)
165  {
166  bool coloredPolygon = true;
167  for(unsigned int j=0; j<mesh->polygons[i].vertices.size(); ++j)
168  {
169  if(!coloredPts.at(mesh->polygons[i].vertices[j]))
170  {
171  coloredPolygon = false;
172  break;
173  }
174  }
175  if(coloredPolygon)
176  {
177  filteredPolygons[oi++] = mesh->polygons[i];
178  }
179  }
180  filteredPolygons.resize(oi);
181  mesh->polygons = filteredPolygons;
182  }
183  }
184 
185  if(minClusterSize)
186  {
187  if(progressState) progressState->callback(uFormat("Filter small polygon clusters..."));
188 
189  // filter polygons
190  std::vector<std::set<int> > neighbors;
191  std::vector<std::set<int> > vertexToPolygons;
192  util3d::createPolygonIndexes(mesh->polygons,
193  mesh->cloud.height*mesh->cloud.width,
194  neighbors,
195  vertexToPolygons);
196  std::list<std::list<int> > clusters = util3d::clusterPolygons(
197  neighbors,
198  minClusterSize<0?0:minClusterSize);
199 
200  std::vector<pcl::Vertices> filteredPolygons(mesh->polygons.size());
201  if(minClusterSize < 0)
202  {
203  // only keep the biggest cluster
204  std::list<std::list<int> >::iterator biggestClusterIndex = clusters.end();
205  unsigned int biggestClusterSize = 0;
206  for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
207  {
208  if(iter->size() > biggestClusterSize)
209  {
210  biggestClusterIndex = iter;
211  biggestClusterSize = iter->size();
212  }
213  }
214  if(biggestClusterIndex != clusters.end())
215  {
216  int oi=0;
217  for(std::list<int>::iterator jter=biggestClusterIndex->begin(); jter!=biggestClusterIndex->end(); ++jter)
218  {
219  filteredPolygons[oi++] = mesh->polygons.at(*jter);
220  }
221  filteredPolygons.resize(oi);
222  }
223  }
224  else
225  {
226  int oi=0;
227  for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
228  {
229  for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
230  {
231  filteredPolygons[oi++] = mesh->polygons.at(*jter);
232  }
233  }
234  filteredPolygons.resize(oi);
235  }
236 
237  int before = (int)mesh->polygons.size();
238  mesh->polygons = filteredPolygons;
239 
240  if(progressState) progressState->callback(uFormat("Filtered %d polygons.", before-(int)mesh->polygons.size()));
241  }
242 
243  // compute normals for the mesh if not already here
244  if(!hasNormals)
245  {
246  // use polygons
247  if(hasColors)
248  {
249  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
250  pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
251 
252  Eigen::Vector3f normal(1,0,0);
253  for(unsigned int i=0; i<mesh->polygons.size(); ++i)
254  {
255  pcl::Vertices & v = mesh->polygons[i];
256  if(!hasNormals)
257  {
258  UASSERT(v.vertices.size()>2);
259  Eigen::Vector3f v0(
260  cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
261  cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
262  cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
263  int last = v.vertices.size()-1;
264  Eigen::Vector3f v1(
265  cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
266  cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
267  cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
268  normal = v0.cross(v1);
269  normal.normalize();
270  }
271  // flat normal (per face)
272  for(unsigned int j=0; j<v.vertices.size(); ++j)
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  }
279  pcl::toPCLPointCloud2 (*cloud, mesh->cloud);
280  }
281  else
282  {
283  pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>);
284  pcl::fromPCLPointCloud2(mesh->cloud, *cloud);
285 
286  for(unsigned int i=0; i<mesh->polygons.size(); ++i)
287  {
288  pcl::Vertices & v = mesh->polygons[i];
289  UASSERT(v.vertices.size()>2);
290  Eigen::Vector3f v0(
291  cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x,
292  cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y,
293  cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z);
294  int last = v.vertices.size()-1;
295  Eigen::Vector3f v1(
296  cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x,
297  cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y,
298  cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z);
299  Eigen::Vector3f normal = v0.cross(v1);
300  normal.normalize();
301  // flat normal (per face)
302  for(unsigned int j=0; j<v.vertices.size(); ++j)
303  {
304  cloud->at(v.vertices[j]).normal_x = normal[0];
305  cloud->at(v.vertices[j]).normal_y = normal[1];
306  cloud->at(v.vertices[j]).normal_z = normal[2];
307  }
308  }
309  pcl::toPCLPointCloud2 (*cloud, mesh->cloud);
310  }
311  }
312 }
313 
314 template<typename PointT>
315 bool intersectRayMesh(
316  const Eigen::Vector3f & origin,
317  const Eigen::Vector3f & dir,
318  const typename pcl::PointCloud<PointT> & cloud,
319  const std::vector<pcl::Vertices> & polygons,
320  bool ignoreBackFaces,
321  float & distance,
322  Eigen::Vector3f & normal,
323  int & index)
324 {
325  bool intersect = false;
326  distance = std::numeric_limits<float>::max();
327  for (size_t i = 0; i < polygons.size(); ++i)
328  {
329  const pcl::Vertices & vert = polygons.at(i);
330  UASSERT(vert.vertices.size()==3);
331  float d;
332  Eigen::Vector3f n;
333  if (intersectRayTriangle(origin, dir,
334  cloud.at(vert.vertices.at(0)).getVector3fMap(),
335  cloud.at(vert.vertices.at(1)).getVector3fMap(),
336  cloud.at(vert.vertices.at(2)).getVector3fMap(), d, n) &&
337  d < distance &&
338  (!ignoreBackFaces || n.dot(dir)<0))
339  {
340  distance = d;
341  index = i;
342  normal = n;
343  intersect |= true;
344  }
345  }
346  normal.normalize();
347  return intersect;
348 }
349 
350 }
351 
352 }
353 
354 
355 #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:315
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
factor
DecisionTreeFactor factor(D &C &B &A, "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0")
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:3904
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:3889
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 Mon Apr 28 2025 02:46:07