Point Cloud Library (PCL)  1.11.1-dev
texture_mapping.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * $Id$
35  *
36  */
37 
38 #ifndef PCL_SURFACE_IMPL_TEXTURE_MAPPING_HPP_
39 #define PCL_SURFACE_IMPL_TEXTURE_MAPPING_HPP_
40 
41 #include <pcl/common/distances.h>
42 #include <pcl/surface/texture_mapping.h>
43 #include <unordered_set>
44 
45 ///////////////////////////////////////////////////////////////////////////////////////////////
46 template<typename PointInT> std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >
48  const Eigen::Vector3f &p1,
49  const Eigen::Vector3f &p2,
50  const Eigen::Vector3f &p3)
51 {
52  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > tex_coordinates;
53  // process for each face
54  Eigen::Vector3f p1p2 (p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]);
55  Eigen::Vector3f p1p3 (p3[0] - p1[0], p3[1] - p1[1], p3[2] - p1[2]);
56  Eigen::Vector3f p2p3 (p3[0] - p2[0], p3[1] - p2[1], p3[2] - p2[2]);
57 
58  // Normalize
59  p1p2 /= std::sqrt (p1p2.dot (p1p2));
60  p1p3 /= std::sqrt (p1p3.dot (p1p3));
61  p2p3 /= std::sqrt (p2p3.dot (p2p3));
62 
63  // compute vector normal of a face
64  Eigen::Vector3f f_normal = p1p2.cross (p1p3);
65  f_normal /= std::sqrt (f_normal.dot (f_normal));
66 
67  // project vector field onto the face: vector v1_projected = v1 - Dot(v1, n) * n;
68  Eigen::Vector3f f_vector_field = vector_field_ - vector_field_.dot (f_normal) * f_normal;
69 
70  // Normalize
71  f_vector_field /= std::sqrt (f_vector_field.dot (f_vector_field));
72 
73  // texture coordinates
74  Eigen::Vector2f tp1, tp2, tp3;
75 
76  double alpha = std::acos (f_vector_field.dot (p1p2));
77 
78  // distance between 3 vertices of triangles
79  double e1 = (p2 - p3).norm () / f_;
80  double e2 = (p1 - p3).norm () / f_;
81  double e3 = (p1 - p2).norm () / f_;
82 
83  // initialize
84  tp1[0] = 0.0;
85  tp1[1] = 0.0;
86 
87  tp2[0] = static_cast<float> (e3);
88  tp2[1] = 0.0;
89 
90  // determine texture coordinate tp3;
91  double cos_p1 = (e2 * e2 + e3 * e3 - e1 * e1) / (2 * e2 * e3);
92  double sin_p1 = sqrt (1 - (cos_p1 * cos_p1));
93 
94  tp3[0] = static_cast<float> (cos_p1 * e2);
95  tp3[1] = static_cast<float> (sin_p1 * e2);
96 
97  // rotating by alpha (angle between V and pp1 & pp2)
98  Eigen::Vector2f r_tp2, r_tp3;
99  r_tp2[0] = static_cast<float> (tp2[0] * std::cos (alpha) - tp2[1] * std::sin (alpha));
100  r_tp2[1] = static_cast<float> (tp2[0] * std::sin (alpha) + tp2[1] * std::cos (alpha));
101 
102  r_tp3[0] = static_cast<float> (tp3[0] * std::cos (alpha) - tp3[1] * std::sin (alpha));
103  r_tp3[1] = static_cast<float> (tp3[0] * std::sin (alpha) + tp3[1] * std::cos (alpha));
104 
105  // shifting
106  tp1[0] = tp1[0];
107  tp2[0] = r_tp2[0];
108  tp3[0] = r_tp3[0];
109  tp1[1] = tp1[1];
110  tp2[1] = r_tp2[1];
111  tp3[1] = r_tp3[1];
112 
113  float min_x = tp1[0];
114  float min_y = tp1[1];
115  if (min_x > tp2[0])
116  min_x = tp2[0];
117  if (min_x > tp3[0])
118  min_x = tp3[0];
119  if (min_y > tp2[1])
120  min_y = tp2[1];
121  if (min_y > tp3[1])
122  min_y = tp3[1];
123 
124  if (min_x < 0)
125  {
126  tp1[0] -= min_x;
127  tp2[0] -= min_x;
128  tp3[0] -= min_x;
129  }
130  if (min_y < 0)
131  {
132  tp1[1] -= min_y;
133  tp2[1] -= min_y;
134  tp3[1] -= min_y;
135  }
136 
137  tex_coordinates.push_back (tp1);
138  tex_coordinates.push_back (tp2);
139  tex_coordinates.push_back (tp3);
140  return (tex_coordinates);
141 }
142 
143 ///////////////////////////////////////////////////////////////////////////////////////////////
144 template<typename PointInT> void
146 {
147  // mesh information
148  int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height;
149  int point_size = static_cast<int> (tex_mesh.cloud.data.size ()) / nr_points;
150 
151  // temporary PointXYZ
152  float x, y, z;
153  // temporary face
154  Eigen::Vector3f facet[3];
155 
156  // texture coordinates for each mesh
157  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >texture_map;
158 
159  for (std::size_t m = 0; m < tex_mesh.tex_polygons.size (); ++m)
160  {
161  // texture coordinates for each mesh
162  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
163 
164  // processing for each face
165  for (std::size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i)
166  {
167  std::size_t idx;
168 
169  // get facet information
170  for (std::size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
171  {
172  idx = tex_mesh.tex_polygons[m][i].vertices[j];
173  memcpy (&x, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float));
174  memcpy (&y, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float));
175  memcpy (&z, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float));
176  facet[j][0] = x;
177  facet[j][1] = y;
178  facet[j][2] = z;
179  }
180 
181  // get texture coordinates of each face
182  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > tex_coordinates = mapTexture2Face (facet[0], facet[1], facet[2]);
183  for (const auto &tex_coordinate : tex_coordinates)
184  texture_map_tmp.push_back (tex_coordinate);
185  }// end faces
186 
187  // texture materials
188  tex_material_.tex_name = "material_" + std::to_string(m);
189  tex_material_.tex_file = tex_files_[m];
190  tex_mesh.tex_materials.push_back (tex_material_);
191 
192  // texture coordinates
193  tex_mesh.tex_coordinates.push_back (texture_map_tmp);
194  }// end meshes
195 }
196 
197 ///////////////////////////////////////////////////////////////////////////////////////////////
198 template<typename PointInT> void
200 {
201  // mesh information
202  int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height;
203  int point_size = static_cast<int> (tex_mesh.cloud.data.size ()) / nr_points;
204 
205  float x_lowest = 100000;
206  float x_highest = 0;
207  float y_lowest = 100000;
208  //float y_highest = 0 ;
209  float z_lowest = 100000;
210  float z_highest = 0;
211  float x_, y_, z_;
212 
213  for (int i = 0; i < nr_points; ++i)
214  {
215  memcpy (&x_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float));
216  memcpy (&y_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float));
217  memcpy (&z_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float));
218  // x
219  if (x_ <= x_lowest)
220  x_lowest = x_;
221  if (x_ > x_lowest)
222  x_highest = x_;
223 
224  // y
225  if (y_ <= y_lowest)
226  y_lowest = y_;
227  //if (y_ > y_lowest) y_highest = y_;
228 
229  // z
230  if (z_ <= z_lowest)
231  z_lowest = z_;
232  if (z_ > z_lowest)
233  z_highest = z_;
234  }
235  // x
236  float x_range = (x_lowest - x_highest) * -1;
237  float x_offset = 0 - x_lowest;
238  // x
239  // float y_range = (y_lowest - y_highest)*-1;
240  // float y_offset = 0 - y_lowest;
241  // z
242  float z_range = (z_lowest - z_highest) * -1;
243  float z_offset = 0 - z_lowest;
244 
245  // texture coordinates for each mesh
246  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >texture_map;
247 
248  for (std::size_t m = 0; m < tex_mesh.tex_polygons.size (); ++m)
249  {
250  // texture coordinates for each mesh
251  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
252 
253  // processing for each face
254  for (std::size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i)
255  {
256  Eigen::Vector2f tmp_VT;
257  for (std::size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
258  {
259  std::size_t idx = tex_mesh.tex_polygons[m][i].vertices[j];
260  memcpy (&x_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float));
261  memcpy (&y_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float));
262  memcpy (&z_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float));
263 
264  // calculate uv coordinates
265  tmp_VT[0] = (x_ + x_offset) / x_range;
266  tmp_VT[1] = (z_ + z_offset) / z_range;
267  texture_map_tmp.push_back (tmp_VT);
268  }
269  }// end faces
270 
271  // texture materials
272  tex_material_.tex_name = "material_" + std::to_string(m);
273  tex_material_.tex_file = tex_files_[m];
274  tex_mesh.tex_materials.push_back (tex_material_);
275 
276  // texture coordinates
277  tex_mesh.tex_coordinates.push_back (texture_map_tmp);
278  }// end meshes
279 }
280 
281 ///////////////////////////////////////////////////////////////////////////////////////////////
282 template<typename PointInT> void
284 {
285 
286  if (tex_mesh.tex_polygons.size () != cams.size () + 1)
287  {
288  PCL_ERROR ("The mesh should be divided into nbCamera+1 sub-meshes.\n");
289  PCL_ERROR ("You provided %d cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.tex_polygons.size ());
290  return;
291  }
292 
293  PCL_INFO ("You provided %d cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.tex_polygons.size ());
294 
295  typename pcl::PointCloud<PointInT>::Ptr originalCloud (new pcl::PointCloud<PointInT>);
296  typename pcl::PointCloud<PointInT>::Ptr camera_transformed_cloud (new pcl::PointCloud<PointInT>);
297 
298  // convert mesh's cloud to pcl format for ease
299  pcl::fromPCLPointCloud2 (tex_mesh.cloud, *originalCloud);
300 
301  for (std::size_t m = 0; m < cams.size (); ++m)
302  {
303  // get current camera parameters
304  Camera current_cam = cams[m];
305 
306  // get camera transform
307  Eigen::Affine3f cam_trans = current_cam.pose;
308 
309  // transform cloud into current camera frame
310  pcl::transformPointCloud (*originalCloud, *camera_transformed_cloud, cam_trans.inverse ());
311 
312  // vector of texture coordinates for each face
313  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
314 
315  // processing each face visible by this camera
316  for (const auto &tex_polygon : tex_mesh.tex_polygons[m])
317  {
318  Eigen::Vector2f tmp_VT;
319  // for each point of this face
320  for (const auto &vertex : tex_polygon.vertices)
321  {
322  // get point
323  PointInT pt = (*camera_transformed_cloud)[vertex];
324 
325  // compute UV coordinates for this point
326  getPointUVCoordinates (pt, current_cam, tmp_VT);
327  texture_map_tmp.push_back (tmp_VT);
328  }// end points
329  }// end faces
330 
331  // texture materials
332  tex_material_.tex_name = "material_" + std::to_string(m);
333  tex_material_.tex_file = current_cam.texture_file;
334  tex_mesh.tex_materials.push_back (tex_material_);
335 
336  // texture coordinates
337  tex_mesh.tex_coordinates.push_back (texture_map_tmp);
338  }// end cameras
339 
340  // push on extra empty UV map (for unseen faces) so that obj writer does not crash!
341  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
342  for (const auto &tex_polygon : tex_mesh.tex_polygons[cams.size ()])
343  for (std::size_t j = 0; j < tex_polygon.vertices.size (); ++j)
344  {
345  Eigen::Vector2f tmp_VT;
346  tmp_VT[0] = -1;
347  tmp_VT[1] = -1;
348  texture_map_tmp.push_back (tmp_VT);
349  }
350 
351  tex_mesh.tex_coordinates.push_back (texture_map_tmp);
352 
353  // push on an extra dummy material for the same reason
354  tex_material_.tex_name = "material_" + std::to_string(cams.size());
355  tex_material_.tex_file = "occluded.jpg";
356  tex_mesh.tex_materials.push_back (tex_material_);
357 }
358 
359 ///////////////////////////////////////////////////////////////////////////////////////////////
360 template<typename PointInT> bool
362 {
363  Eigen::Vector3f direction;
364  direction (0) = pt.x;
365  direction (1) = pt.y;
366  direction (2) = pt.z;
367 
368  pcl::Indices indices;
369 
370  PointCloudConstPtr cloud (new PointCloud());
371  cloud = octree->getInputCloud();
372 
373  double distance_threshold = octree->getResolution();
374 
375  // raytrace
376  octree->getIntersectedVoxelIndices(direction, -direction, indices);
377 
378  int nbocc = static_cast<int> (indices.size ());
379  for (const auto &index : indices)
380  {
381  // if intersected point is on the over side of the camera
382  if (pt.z * (*cloud)[index].z < 0)
383  {
384  nbocc--;
385  continue;
386  }
387 
388  if (std::fabs ((*cloud)[index].z - pt.z) <= distance_threshold)
389  {
390  // points are very close to each-other, we do not consider the occlusion
391  nbocc--;
392  }
393  }
394 
395  return (nbocc != 0);
396 }
397 
398 ///////////////////////////////////////////////////////////////////////////////////////////////
399 template<typename PointInT> void
401  PointCloudPtr &filtered_cloud,
402  const double octree_voxel_size, pcl::Indices &visible_indices,
403  pcl::Indices &occluded_indices)
404 {
405  // variable used to filter occluded points by depth
406  double maxDeltaZ = octree_voxel_size;
407 
408  // create an octree to perform rayTracing
409  Octree octree (octree_voxel_size);
410  // create octree structure
411  octree.setInputCloud (input_cloud);
412  // update bounding box automatically
413  octree.defineBoundingBox ();
414  // add points in the tree
415  octree.addPointsFromInputCloud ();
416 
417  visible_indices.clear ();
418 
419  // for each point of the cloud, raycast toward camera and check intersected voxels.
420  Eigen::Vector3f direction;
421  pcl::Indices indices;
422  for (std::size_t i = 0; i < input_cloud->size (); ++i)
423  {
424  direction (0) = (*input_cloud)[i].x;
425  direction (1) = (*input_cloud)[i].y;
426  direction (2) = (*input_cloud)[i].z;
427 
428  // if point is not occluded
429  octree.getIntersectedVoxelIndices (direction, -direction, indices);
430 
431  int nbocc = static_cast<int> (indices.size ());
432  for (const auto &index : indices)
433  {
434  // if intersected point is on the over side of the camera
435  if ((*input_cloud)[i].z * (*input_cloud)[index].z < 0)
436  {
437  nbocc--;
438  continue;
439  }
440 
441  if (std::fabs ((*input_cloud)[index].z - (*input_cloud)[i].z) <= maxDeltaZ)
442  {
443  // points are very close to each-other, we do not consider the occlusion
444  nbocc--;
445  }
446  }
447 
448  if (nbocc == 0)
449  {
450  // point is added in the filtered mesh
451  filtered_cloud->points.push_back ((*input_cloud)[i]);
452  visible_indices.push_back (static_cast<pcl::index_t> (i));
453  }
454  else
455  {
456  occluded_indices.push_back (static_cast<pcl::index_t> (i));
457  }
458  }
459 
460 }
461 
462 ///////////////////////////////////////////////////////////////////////////////////////////////
463 template<typename PointInT> void
464 pcl::TextureMapping<PointInT>::removeOccludedPoints (const pcl::TextureMesh &tex_mesh, pcl::TextureMesh &cleaned_mesh, const double octree_voxel_size)
465 {
466  // copy mesh
467  cleaned_mesh = tex_mesh;
468 
470  typename pcl::PointCloud<PointInT>::Ptr filtered_cloud (new pcl::PointCloud<PointInT>);
471 
472  // load points into a PCL format
473  pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);
474 
475  pcl::Indices visible, occluded;
476  removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded);
477 
478  // Now that we know which points are visible, let's iterate over each face.
479  // if the face has one invisible point => out!
480  for (std::size_t polygons = 0; polygons < cleaned_mesh.tex_polygons.size (); ++polygons)
481  {
482  // remove all faces from cleaned mesh
483  cleaned_mesh.tex_polygons[polygons].clear ();
484  // iterate over faces
485  for (std::size_t faces = 0; faces < tex_mesh.tex_polygons[polygons].size (); ++faces)
486  {
487  // check if all the face's points are visible
488  bool faceIsVisible = true;
489 
490  // iterate over face's vertex
491  for (const auto &vertex : tex_mesh.tex_polygons[polygons][faces].vertices)
492  {
493  if (find (occluded.begin (), occluded.end (), vertex) == occluded.end ())
494  {
495  // point is not in the occluded vector
496  // PCL_INFO (" VISIBLE!\n");
497  }
498  else
499  {
500  // point was occluded
501  // PCL_INFO(" OCCLUDED!\n");
502  faceIsVisible = false;
503  }
504  }
505 
506  if (faceIsVisible)
507  {
508  cleaned_mesh.tex_polygons[polygons].push_back (tex_mesh.tex_polygons[polygons][faces]);
509  }
510 
511  }
512  }
513 }
514 
515 ///////////////////////////////////////////////////////////////////////////////////////////////
516 template<typename PointInT> void
518  const double octree_voxel_size)
519 {
520  PointCloudPtr cloud (new PointCloud);
521 
522  // load points into a PCL format
523  pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);
524 
525  pcl::Indices visible, occluded;
526  removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded);
527 
528 }
529 
530 ///////////////////////////////////////////////////////////////////////////////////////////////
531 template<typename PointInT> int
533  const pcl::texture_mapping::CameraVector &cameras, const double octree_voxel_size,
534  PointCloud &visible_pts)
535 {
536  if (tex_mesh.tex_polygons.size () != 1)
537  {
538  PCL_ERROR ("The mesh must contain only 1 sub-mesh!\n");
539  return (-1);
540  }
541 
542  if (cameras.empty ())
543  {
544  PCL_ERROR ("Must provide at least one camera info!\n");
545  return (-1);
546  }
547 
548  // copy mesh
549  sorted_mesh = tex_mesh;
550  // clear polygons from cleaned_mesh
551  sorted_mesh.tex_polygons.clear ();
552 
553  typename pcl::PointCloud<PointInT>::Ptr original_cloud (new pcl::PointCloud<PointInT>);
554  typename pcl::PointCloud<PointInT>::Ptr transformed_cloud (new pcl::PointCloud<PointInT>);
555  typename pcl::PointCloud<PointInT>::Ptr filtered_cloud (new pcl::PointCloud<PointInT>);
556 
557  // load points into a PCL format
558  pcl::fromPCLPointCloud2 (tex_mesh.cloud, *original_cloud);
559 
560  // for each camera
561  for (const auto &camera : cameras)
562  {
563  // get camera pose as transform
564  Eigen::Affine3f cam_trans = camera.pose;
565 
566  // transform original cloud in camera coordinates
567  pcl::transformPointCloud (*original_cloud, *transformed_cloud, cam_trans.inverse ());
568 
569  // find occlusions on transformed cloud
570  pcl::Indices visible, occluded;
571  removeOccludedPoints (transformed_cloud, filtered_cloud, octree_voxel_size, visible, occluded);
572  visible_pts = *filtered_cloud;
573 
574  // pushing occluded idxs into a set for faster lookup
575  std::unordered_set<index_t> occluded_set(occluded.cbegin(), occluded.cend());
576 
577  // find visible faces => add them to polygon N for camera N
578  // add polygon group for current camera in clean
579  std::vector<pcl::Vertices> visibleFaces_currentCam;
580  // iterate over the faces of the current mesh
581  for (std::size_t faces = 0; faces < tex_mesh.tex_polygons[0].size (); ++faces)
582  {
583  // check if all the face's points are visible
584  // iterate over face's vertex
585  const auto faceIsVisible = std::all_of(tex_mesh.tex_polygons[0][faces].vertices.cbegin(),
586  tex_mesh.tex_polygons[0][faces].vertices.cend(),
587  [&](const auto& vertex)
588  {
589  if (occluded_set.find(vertex) != occluded_set.cend()) {
590  return false; // point is occluded
591  }
592  // is the point visible to the camera?
593  Eigen::Vector2f dummy_UV;
594  return this->getPointUVCoordinates ((*transformed_cloud)[vertex], camera, dummy_UV);
595  });
596 
597  if (faceIsVisible)
598  {
599  // push current visible face into the sorted mesh
600  visibleFaces_currentCam.push_back (tex_mesh.tex_polygons[0][faces]);
601  // remove it from the unsorted mesh
602  tex_mesh.tex_polygons[0].erase (tex_mesh.tex_polygons[0].begin () + faces);
603  faces--;
604  }
605 
606  }
607  sorted_mesh.tex_polygons.push_back (visibleFaces_currentCam);
608  }
609 
610  // we should only have occluded and non-visible faces left in tex_mesh.tex_polygons[0]
611  // we need to add them as an extra polygon in the sorted mesh
612  sorted_mesh.tex_polygons.push_back (tex_mesh.tex_polygons[0]);
613  return (0);
614 }
615 
616 ///////////////////////////////////////////////////////////////////////////////////////////////
617 template<typename PointInT> void
620  const double octree_voxel_size, const bool show_nb_occlusions,
621  const int max_occlusions)
622  {
623  // variable used to filter occluded points by depth
624  double maxDeltaZ = octree_voxel_size * 2.0;
625 
626  // create an octree to perform rayTracing
627  Octree octree (octree_voxel_size);
628  // create octree structure
629  octree.setInputCloud (input_cloud);
630  // update bounding box automatically
631  octree.defineBoundingBox ();
632  // add points in the tree
633  octree.addPointsFromInputCloud ();
634 
635  // ray direction
636  Eigen::Vector3f direction;
637 
638  pcl::Indices indices;
639  // point from where we ray-trace
640  pcl::PointXYZI pt;
641 
642  std::vector<double> zDist;
643  std::vector<double> ptDist;
644  // for each point of the cloud, ray-trace toward the camera and check intersected voxels.
645  for (const auto& point: *input_cloud)
646  {
647  direction = pt.getVector3fMap() = point.getVector3fMap();
648 
649  // get number of occlusions for that point
650  indices.clear ();
651  int nbocc = octree.getIntersectedVoxelIndices (direction, -direction, indices);
652 
653  nbocc = static_cast<int> (indices.size ());
654 
655  // TODO need to clean this up and find tricks to get remove aliasaing effect on planes
656  for (const auto &index : indices)
657  {
658  // if intersected point is on the over side of the camera
659  if (pt.z * (*input_cloud)[index].z < 0)
660  {
661  nbocc--;
662  }
663  else if (std::fabs ((*input_cloud)[index].z - pt.z) <= maxDeltaZ)
664  {
665  // points are very close to each-other, we do not consider the occlusion
666  nbocc--;
667  }
668  else
669  {
670  zDist.push_back (std::fabs ((*input_cloud)[index].z - pt.z));
671  ptDist.push_back (pcl::euclideanDistance ((*input_cloud)[index], pt));
672  }
673  }
674 
675  if (show_nb_occlusions)
676  (nbocc <= max_occlusions) ? (pt.intensity = static_cast<float> (nbocc)) : (pt.intensity = static_cast<float> (max_occlusions));
677  else
678  (nbocc == 0) ? (pt.intensity = 0) : (pt.intensity = 1);
679 
680  colored_cloud->points.push_back (pt);
681  }
682 
683  if (zDist.size () >= 2)
684  {
685  std::sort (zDist.begin (), zDist.end ());
686  std::sort (ptDist.begin (), ptDist.end ());
687  }
688 }
689 
690 ///////////////////////////////////////////////////////////////////////////////////////////////
691 template<typename PointInT> void
693  double octree_voxel_size, bool show_nb_occlusions, int max_occlusions)
694 {
695  // load points into a PCL format
697  pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);
698 
699  showOcclusions (cloud, colored_cloud, octree_voxel_size, show_nb_occlusions, max_occlusions);
700 }
701 
702 ///////////////////////////////////////////////////////////////////////////////////////////////
703 template<typename PointInT> void
705 {
706 
707  if (mesh.tex_polygons.size () != 1)
708  return;
709 
711 
712  pcl::fromPCLPointCloud2 (mesh.cloud, *mesh_cloud);
713 
714  std::vector<pcl::Vertices> faces;
715 
716  for (int current_cam = 0; current_cam < static_cast<int> (cameras.size ()); ++current_cam)
717  {
718  PCL_INFO ("Processing camera %d of %d.\n", current_cam+1, cameras.size ());
719 
720  // transform mesh into camera's frame
721  typename pcl::PointCloud<PointInT>::Ptr camera_cloud (new pcl::PointCloud<PointInT>);
722  pcl::transformPointCloud (*mesh_cloud, *camera_cloud, cameras[current_cam].pose.inverse ());
723 
724  // CREATE UV MAP FOR CURRENT FACES
726  std::vector<bool> visibility;
727  visibility.resize (mesh.tex_polygons[current_cam].size ());
728  std::vector<UvIndex> indexes_uv_to_points;
729  // for each current face
730 
731  //TODO change this
732  pcl::PointXY nan_point;
733  nan_point.x = std::numeric_limits<float>::quiet_NaN ();
734  nan_point.y = std::numeric_limits<float>::quiet_NaN ();
735  UvIndex u_null;
736  u_null.idx_cloud = -1;
737  u_null.idx_face = -1;
738 
739  int cpt_invisible=0;
740  for (int idx_face = 0; idx_face < static_cast<int> (mesh.tex_polygons[current_cam].size ()); ++idx_face)
741  {
742  //project each vertice, if one is out of view, stop
743  pcl::PointXY uv_coord1;
744  pcl::PointXY uv_coord2;
745  pcl::PointXY uv_coord3;
746 
747  if (isFaceProjected (cameras[current_cam],
748  (*camera_cloud)[mesh.tex_polygons[current_cam][idx_face].vertices[0]],
749  (*camera_cloud)[mesh.tex_polygons[current_cam][idx_face].vertices[1]],
750  (*camera_cloud)[mesh.tex_polygons[current_cam][idx_face].vertices[2]],
751  uv_coord1,
752  uv_coord2,
753  uv_coord3))
754  {
755  // face is in the camera's FOV
756 
757  // add UV coordinates
758  projections->points.push_back (uv_coord1);
759  projections->points.push_back (uv_coord2);
760  projections->points.push_back (uv_coord3);
761 
762  // remember corresponding face
763  UvIndex u1, u2, u3;
764  u1.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[0];
765  u2.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[1];
766  u3.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[2];
767  u1.idx_face = idx_face; u2.idx_face = idx_face; u3.idx_face = idx_face;
768  indexes_uv_to_points.push_back (u1);
769  indexes_uv_to_points.push_back (u2);
770  indexes_uv_to_points.push_back (u3);
771 
772  //keep track of visibility
773  visibility[idx_face] = true;
774  }
775  else
776  {
777  projections->points.push_back (nan_point);
778  projections->points.push_back (nan_point);
779  projections->points.push_back (nan_point);
780  indexes_uv_to_points.push_back (u_null);
781  indexes_uv_to_points.push_back (u_null);
782  indexes_uv_to_points.push_back (u_null);
783  //keep track of visibility
784  visibility[idx_face] = false;
785  cpt_invisible++;
786  }
787  }
788 
789  // projections contains all UV points of the current faces
790  // indexes_uv_to_points links a uv point to its point in the camera cloud
791  // visibility contains tells if a face was in the camera FOV (false = skip)
792 
793  // TODO handle case were no face could be projected
794  if (visibility.size () - cpt_invisible !=0)
795  {
796  //create kdtree
798  kdtree.setInputCloud (projections);
799 
800  pcl::Indices idxNeighbors;
801  std::vector<float> neighborsSquaredDistance;
802  // af first (idx_pcan < current_cam), check if some of the faces attached to previous cameras occlude the current faces
803  // then (idx_pcam == current_cam), check for self occlusions. At this stage, we skip faces that were already marked as occluded
804  cpt_invisible = 0;
805  for (int idx_pcam = 0 ; idx_pcam <= current_cam ; ++idx_pcam)
806  {
807  // project all faces
808  for (int idx_face = 0; idx_face < static_cast<int> (mesh.tex_polygons[idx_pcam].size ()); ++idx_face)
809  {
810 
811  if (idx_pcam == current_cam && !visibility[idx_face])
812  {
813  // we are now checking for self occlusions within the current faces
814  // the current face was already declared as occluded.
815  // therefore, it cannot occlude another face anymore => we skip it
816  continue;
817  }
818 
819  // project each vertice, if one is out of view, stop
820  pcl::PointXY uv_coord1;
821  pcl::PointXY uv_coord2;
822  pcl::PointXY uv_coord3;
823 
824  if (isFaceProjected (cameras[current_cam],
825  (*camera_cloud)[mesh.tex_polygons[idx_pcam][idx_face].vertices[0]],
826  (*camera_cloud)[mesh.tex_polygons[idx_pcam][idx_face].vertices[1]],
827  (*camera_cloud)[mesh.tex_polygons[idx_pcam][idx_face].vertices[2]],
828  uv_coord1,
829  uv_coord2,
830  uv_coord3))
831  {
832  // face is in the camera's FOV
833  //get its circumsribed circle
834  double radius;
835  pcl::PointXY center;
836  // getTriangleCircumcenterAndSize (uv_coord1, uv_coord2, uv_coord3, center, radius);
837  getTriangleCircumcscribedCircleCentroid(uv_coord1, uv_coord2, uv_coord3, center, radius); // this function yields faster results than getTriangleCircumcenterAndSize
838 
839  // get points inside circ.circle
840  if (kdtree.radiusSearch (center, radius, idxNeighbors, neighborsSquaredDistance) > 0 )
841  {
842  // for each neighbor
843  for (const auto &idxNeighbor : idxNeighbors)
844  {
845  if (std::max ((*camera_cloud)[mesh.tex_polygons[idx_pcam][idx_face].vertices[0]].z,
846  std::max ((*camera_cloud)[mesh.tex_polygons[idx_pcam][idx_face].vertices[1]].z,
847  (*camera_cloud)[mesh.tex_polygons[idx_pcam][idx_face].vertices[2]].z))
848  < (*camera_cloud)[indexes_uv_to_points[idxNeighbor].idx_cloud].z)
849  {
850  // neighbor is farther than all the face's points. Check if it falls into the triangle
851  if (checkPointInsideTriangle(uv_coord1, uv_coord2, uv_coord3, (*projections)[idxNeighbor]))
852  {
853  // current neighbor is inside triangle and is closer => the corresponding face
854  visibility[indexes_uv_to_points[idxNeighbor].idx_face] = false;
855  cpt_invisible++;
856  //TODO we could remove the projections of this face from the kd-tree cloud, but I fond it slower, and I need the point to keep ordered to querry UV coordinates later
857  }
858  }
859  }
860  }
861  }
862  }
863  }
864  }
865 
866  // now, visibility is true for each face that belongs to the current camera
867  // if a face is not visible, we push it into the next one.
868 
869  if (static_cast<int> (mesh.tex_coordinates.size ()) <= current_cam)
870  {
871  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > dummy_container;
872  mesh.tex_coordinates.push_back (dummy_container);
873  }
874  mesh.tex_coordinates[current_cam].resize (3 * visibility.size ());
875 
876  std::vector<pcl::Vertices> occluded_faces;
877  occluded_faces.resize (visibility.size ());
878  std::vector<pcl::Vertices> visible_faces;
879  visible_faces.resize (visibility.size ());
880 
881  int cpt_occluded_faces = 0;
882  int cpt_visible_faces = 0;
883 
884  for (std::size_t idx_face = 0 ; idx_face < visibility.size () ; ++idx_face)
885  {
886  if (visibility[idx_face])
887  {
888  // face is visible by the current camera copy UV coordinates
889  mesh.tex_coordinates[current_cam][cpt_visible_faces * 3](0) = (*projections)[idx_face*3].x;
890  mesh.tex_coordinates[current_cam][cpt_visible_faces * 3](1) = (*projections)[idx_face*3].y;
891 
892  mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](0) = (*projections)[idx_face*3 + 1].x;
893  mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](1) = (*projections)[idx_face*3 + 1].y;
894 
895  mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](0) = (*projections)[idx_face*3 + 2].x;
896  mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](1) = (*projections)[idx_face*3 + 2].y;
897 
898  visible_faces[cpt_visible_faces] = mesh.tex_polygons[current_cam][idx_face];
899 
900  cpt_visible_faces++;
901  }
902  else
903  {
904  // face is occluded copy face into temp vector
905  occluded_faces[cpt_occluded_faces] = mesh.tex_polygons[current_cam][idx_face];
906  cpt_occluded_faces++;
907  }
908  }
909  mesh.tex_coordinates[current_cam].resize (cpt_visible_faces*3);
910 
911  occluded_faces.resize (cpt_occluded_faces);
912  mesh.tex_polygons.push_back (occluded_faces);
913 
914  visible_faces.resize (cpt_visible_faces);
915  mesh.tex_polygons[current_cam].clear ();
916  mesh.tex_polygons[current_cam] = visible_faces;
917  }
918 
919  // we have been through all the cameras.
920  // if any faces are left, they were not visible by any camera
921  // we still need to produce uv coordinates for them
922 
923  if (mesh.tex_coordinates.size() <= cameras.size ())
924  {
925  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > dummy_container;
926  mesh.tex_coordinates.push_back(dummy_container);
927  }
928 
929 
930  for(std::size_t idx_face = 0 ; idx_face < mesh.tex_polygons[cameras.size()].size() ; ++idx_face)
931  {
932  Eigen::Vector2f UV1, UV2, UV3;
933  UV1(0) = -1.0; UV1(1) = -1.0;
934  UV2(0) = -1.0; UV2(1) = -1.0;
935  UV3(0) = -1.0; UV3(1) = -1.0;
936  mesh.tex_coordinates[cameras.size()].push_back(UV1);
937  mesh.tex_coordinates[cameras.size()].push_back(UV2);
938  mesh.tex_coordinates[cameras.size()].push_back(UV3);
939  }
940 
941 }
942 
943 ///////////////////////////////////////////////////////////////////////////////////////////////
944 template<typename PointInT> inline void
946 {
947  // we simplify the problem by translating the triangle's origin to its first point
948  pcl::PointXY ptB, ptC;
949  ptB.x = p2.x - p1.x; ptB.y = p2.y - p1.y; // B'=B-A
950  ptC.x = p3.x - p1.x; ptC.y = p3.y - p1.y; // C'=C-A
951 
952  double D = 2.0*(ptB.x*ptC.y - ptB.y*ptC.x); // D'=2(B'x*C'y - B'y*C'x)
953 
954  // Safety check to avoid division by zero
955  if(D == 0)
956  {
957  circomcenter.x = p1.x;
958  circomcenter.y = p1.y;
959  }
960  else
961  {
962  // compute squares once
963  double bx2 = ptB.x * ptB.x; // B'x^2
964  double by2 = ptB.y * ptB.y; // B'y^2
965  double cx2 = ptC.x * ptC.x; // C'x^2
966  double cy2 = ptC.y * ptC.y; // C'y^2
967 
968  // compute circomcenter's coordinates (translate back to original coordinates)
969  circomcenter.x = static_cast<float> (p1.x + (ptC.y*(bx2 + by2) - ptB.y*(cx2 + cy2)) / D);
970  circomcenter.y = static_cast<float> (p1.y + (ptB.x*(cx2 + cy2) - ptC.x*(bx2 + by2)) / D);
971  }
972 
973  radius = std::sqrt( (circomcenter.x - p1.x)*(circomcenter.x - p1.x) + (circomcenter.y - p1.y)*(circomcenter.y - p1.y));//2.0* (p1.x*(p2.y - p3.y) + p2.x*(p3.y - p1.y) + p3.x*(p1.y - p2.y));
974 }
975 
976 ///////////////////////////////////////////////////////////////////////////////////////////////
977 template<typename PointInT> inline void
979 {
980  // compute centroid's coordinates (translate back to original coordinates)
981  circumcenter.x = static_cast<float> (p1.x + p2.x + p3.x ) / 3;
982  circumcenter.y = static_cast<float> (p1.y + p2.y + p3.y ) / 3;
983  double r1 = (circumcenter.x - p1.x) * (circumcenter.x - p1.x) + (circumcenter.y - p1.y) * (circumcenter.y - p1.y) ;
984  double r2 = (circumcenter.x - p2.x) * (circumcenter.x - p2.x) + (circumcenter.y - p2.y) * (circumcenter.y - p2.y) ;
985  double r3 = (circumcenter.x - p3.x) * (circumcenter.x - p3.x) + (circumcenter.y - p3.y) * (circumcenter.y - p3.y) ;
986 
987  // radius
988  radius = std::sqrt( std::max( r1, std::max( r2, r3) )) ;
989 }
990 
991 
992 ///////////////////////////////////////////////////////////////////////////////////////////////
993 template<typename PointInT> inline bool
994 pcl::TextureMapping<PointInT>::getPointUVCoordinates(const PointInT &pt, const Camera &cam, pcl::PointXY &UV_coordinates)
995 {
996  if (pt.z > 0)
997  {
998  // compute image center and dimension
999  double sizeX = cam.width;
1000  double sizeY = cam.height;
1001  double cx, cy;
1002  if (cam.center_w > 0)
1003  cx = cam.center_w;
1004  else
1005  cx = sizeX / 2.0;
1006  if (cam.center_h > 0)
1007  cy = cam.center_h;
1008  else
1009  cy = sizeY / 2.0;
1010 
1011  double focal_x, focal_y;
1012  if (cam.focal_length_w > 0)
1013  focal_x = cam.focal_length_w;
1014  else
1015  focal_x = cam.focal_length;
1016  if (cam.focal_length_h > 0)
1017  focal_y = cam.focal_length_h;
1018  else
1019  focal_y = cam.focal_length;
1020 
1021  // project point on camera's image plane
1022  UV_coordinates.x = static_cast<float> ((focal_x * (pt.x / pt.z) + cx) / sizeX); //horizontal
1023  UV_coordinates.y = 1.0f - static_cast<float> ((focal_y * (pt.y / pt.z) + cy) / sizeY); //vertical
1024 
1025  // point is visible!
1026  if (UV_coordinates.x >= 0.0 && UV_coordinates.x <= 1.0 && UV_coordinates.y >= 0.0 && UV_coordinates.y <= 1.0)
1027  return (true); // point was visible by the camera
1028  }
1029 
1030  // point is NOT visible by the camera
1031  UV_coordinates.x = -1.0f;
1032  UV_coordinates.y = -1.0f;
1033  return (false); // point was not visible by the camera
1034 }
1035 
1036 ///////////////////////////////////////////////////////////////////////////////////////////////
1037 template<typename PointInT> inline bool
1039 {
1040  // Compute vectors
1041  Eigen::Vector2d v0, v1, v2;
1042  v0(0) = p3.x - p1.x; v0(1) = p3.y - p1.y; // v0= C - A
1043  v1(0) = p2.x - p1.x; v1(1) = p2.y - p1.y; // v1= B - A
1044  v2(0) = pt.x - p1.x; v2(1) = pt.y - p1.y; // v2= P - A
1045 
1046  // Compute dot products
1047  double dot00 = v0.dot(v0); // dot00 = dot(v0, v0)
1048  double dot01 = v0.dot(v1); // dot01 = dot(v0, v1)
1049  double dot02 = v0.dot(v2); // dot02 = dot(v0, v2)
1050  double dot11 = v1.dot(v1); // dot11 = dot(v1, v1)
1051  double dot12 = v1.dot(v2); // dot12 = dot(v1, v2)
1052 
1053  // Compute barycentric coordinates
1054  double invDenom = 1.0 / (dot00*dot11 - dot01*dot01);
1055  double u = (dot11*dot02 - dot01*dot12) * invDenom;
1056  double v = (dot00*dot12 - dot01*dot02) * invDenom;
1057 
1058  // Check if point is in triangle
1059  return ((u >= 0) && (v >= 0) && (u + v < 1));
1060 }
1061 
1062 ///////////////////////////////////////////////////////////////////////////////////////////////
1063 template<typename PointInT> inline bool
1064 pcl::TextureMapping<PointInT>::isFaceProjected (const Camera &camera, const PointInT &p1, const PointInT &p2, const PointInT &p3, pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3)
1065 {
1066  return (getPointUVCoordinates(p1, camera, proj1)
1067  &&
1068  getPointUVCoordinates(p2, camera, proj2)
1069  &&
1070  getPointUVCoordinates(p3, camera, proj3)
1071  );
1072 }
1073 
1074 #define PCL_INSTANTIATE_TextureMapping(T) \
1075  template class PCL_EXPORTS pcl::TextureMapping<T>;
1076 
1077 #endif /* TEXTURE_MAPPING_HPP_ */
pcl::TextureMapping::checkPointInsideTriangle
bool checkPointInsideTriangle(const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, const pcl::PointXY &pt)
Returns True if a point lays within a triangle.
Definition: texture_mapping.hpp:1038
pcl::TextureMesh::tex_polygons
std::vector< std::vector< pcl::Vertices > > tex_polygons
Definition: TextureMesh.h:94
pcl::texture_mapping::Camera::width
double width
Definition: texture_mapping.h:76
pcl::texture_mapping::Camera::focal_length_w
double focal_length_w
Definition: texture_mapping.h:71
pcl::texture_mapping::Camera::pose
Eigen::Affine3f pose
Definition: texture_mapping.h:69
pcl::octree::OctreePointCloudSearch::getIntersectedVoxelIndices
uindex_t getIntersectedVoxelIndices(Eigen::Vector3f origin, Eigen::Vector3f direction, Indices &k_indices, uindex_t max_voxel_count=0) const
Get indices of all voxels that are intersected by a ray (origin, direction).
Definition: octree_search.hpp:631
pcl::TextureMesh::tex_coordinates
std::vector< std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > > tex_coordinates
Definition: TextureMesh.h:95
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:389
pcl::KdTreeFLANN::radiusSearch
int radiusSearch(const PointT &point, double radius, Indices &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
Search for all the nearest neighbors of the query point in a given radius.
Definition: kdtree_flann.hpp:172
pcl::TextureMapping::getTriangleCircumcscribedCircleCentroid
void getTriangleCircumcscribedCircleCentroid(const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circumcenter, double &radius)
Returns the centroid of a triangle and the corresponding circumscribed circle's radius.
Definition: texture_mapping.hpp:978
pcl::texture_mapping::UvIndex
Structure that links a uv coordinate to its 3D point and face.
Definition: texture_mapping.h:84
pcl::TextureMapping::mapTexture2MeshUV
void mapTexture2MeshUV(pcl::TextureMesh &tex_mesh)
Map texture to a mesh UV mapping.
Definition: texture_mapping.hpp:199
pcl::TextureMapping::textureMeshwithMultipleCameras
void textureMeshwithMultipleCameras(pcl::TextureMesh &mesh, const pcl::texture_mapping::CameraVector &cameras)
Segment and texture faces by camera visibility.
Definition: texture_mapping.hpp:704
pcl::PCLPointCloud2::width
uindex_t width
Definition: PCLPointCloud2.h:21
pcl::texture_mapping::Camera::focal_length_h
double focal_length_h
Definition: texture_mapping.h:72
pcl::octree::OctreePointCloud::addPointsFromInputCloud
void addPointsFromInputCloud()
Add points from input point cloud to octree.
Definition: octree_pointcloud.hpp:79
pcl::euclideanDistance
float euclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the euclidean distance between the two given points.
Definition: distances.h:204
pcl::PointXYZI
Definition: point_types.hpp:466
pcl::TextureMesh::cloud
pcl::PCLPointCloud2 cloud
Definition: TextureMesh.h:90
pcl::PointCloud< PointInT >
pcl::texture_mapping::UvIndex::idx_face
int idx_face
Definition: texture_mapping.h:88
pcl::index_t
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
Definition: types.h:112
pcl::TextureMesh::tex_materials
std::vector< pcl::TexMaterial > tex_materials
Definition: TextureMesh.h:96
pcl::TextureMapping::isFaceProjected
bool isFaceProjected(const Camera &camera, const PointInT &p1, const PointInT &p2, const PointInT &p3, pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3)
Returns true if all the vertices of one face are projected on the camera's image plane.
Definition: texture_mapping.hpp:1064
pcl::octree::OctreePointCloud::defineBoundingBox
void defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree.
Definition: octree_pointcloud.hpp:333
pcl::PCLPointCloud2::height
uindex_t height
Definition: PCLPointCloud2.h:20
pcl::transformPointCloud
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
Definition: transforms.hpp:221
pcl::_PointXYZI::intensity
float intensity
Definition: point_types.hpp:458
pcl::PointXY::x
float x
Definition: point_types.hpp:789
pcl::octree::OctreePointCloud::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud_arg, const IndicesConstPtr &indices_arg=IndicesConstPtr())
Provide a pointer to the input data set.
Definition: octree_pointcloud.h:117
pcl::texture_mapping::Camera::height
double height
Definition: texture_mapping.h:75
pcl::TextureMapping::isPointOccluded
bool isPointOccluded(const PointInT &pt, const OctreePtr octree)
Check if a point is occluded using raycasting on octree.
Definition: texture_mapping.hpp:361
pcl::texture_mapping::Camera::focal_length
double focal_length
Definition: texture_mapping.h:70
pcl::PCLPointCloud2::fields
std::vector<::pcl::PCLPointField > fields
Definition: PCLPointCloud2.h:23
pcl::texture_mapping::Camera
Structure to store camera pose and focal length.
Definition: texture_mapping.h:65
pcl::PointXY::y
float y
Definition: point_types.hpp:790
pcl::TextureMapping::OctreePtr
typename Octree::Ptr OctreePtr
Definition: texture_mapping.h:112
pcl::TextureMapping::getPointUVCoordinates
bool getPointUVCoordinates(const PointInT &pt, const Camera &cam, Eigen::Vector2f &UV_coordinates)
computes UV coordinates of point, observed by one particular camera
Definition: texture_mapping.h:198
pcl::KdTreeFLANN
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:64
pcl::texture_mapping::Camera::texture_file
std::string texture_file
Definition: texture_mapping.h:77
pcl::texture_mapping::Camera::center_w
double center_w
Definition: texture_mapping.h:73
pcl::texture_mapping::CameraVector
std::vector< Camera, Eigen::aligned_allocator< Camera > > CameraVector
Definition: texture_mapping.h:91
pcl::TextureMapping::mapTexture2Mesh
void mapTexture2Mesh(pcl::TextureMesh &tex_mesh)
Map texture to a mesh synthesis algorithm.
Definition: texture_mapping.hpp:145
pcl::PointXY
A 2D point structure representing Euclidean xy coordinates.
Definition: point_types.hpp:787
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
pcl::PCLPointCloud2::data
std::vector< std::uint8_t > data
Definition: PCLPointCloud2.h:30
pcl::TextureMapping::mapTexture2Face
std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > mapTexture2Face(const Eigen::Vector3f &p1, const Eigen::Vector3f &p2, const Eigen::Vector3f &p3)
Map texture to a face.
Definition: texture_mapping.hpp:47
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:407
pcl::TextureMapping::PointCloudConstPtr
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: texture_mapping.h:109
pcl::TextureMapping::showOcclusions
void showOcclusions(const PointCloudPtr &input_cloud, pcl::PointCloud< pcl::PointXYZI >::Ptr &colored_cloud, const double octree_voxel_size, const bool show_nb_occlusions=true, const int max_occlusions=4)
Colors a point cloud, depending on its occlusions.
Definition: texture_mapping.hpp:618
pcl::TextureMapping::removeOccludedPoints
void removeOccludedPoints(const PointCloudPtr &input_cloud, PointCloudPtr &filtered_cloud, const double octree_voxel_size, pcl::Indices &visible_indices, pcl::Indices &occluded_indices)
Remove occluded points from a point cloud.
Definition: texture_mapping.hpp:400
pcl::TextureMesh
Definition: TextureMesh.h:88
pcl::texture_mapping::Camera::center_h
double center_h
Definition: texture_mapping.h:74
distances.h
pcl::TextureMapping::mapMultipleTexturesToMeshUV
void mapMultipleTexturesToMeshUV(pcl::TextureMesh &tex_mesh, pcl::texture_mapping::CameraVector &cams)
Map textures acquired from a set of cameras onto a mesh.
Definition: texture_mapping.hpp:283
pcl::TextureMapping::getTriangleCircumcenterAndSize
void getTriangleCircumcenterAndSize(const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circumcenter, double &radius)
Returns the circumcenter of a triangle and the circle's radius.
Definition: texture_mapping.hpp:945
pcl::TextureMapping::PointCloudPtr
typename PointCloud::Ptr PointCloudPtr
Definition: texture_mapping.h:108
pcl::octree::OctreePointCloudSearch
Octree pointcloud search class
Definition: octree_search.h:57
pcl::KdTreeFLANN::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
Definition: kdtree_flann.hpp:92
pcl::fromPCLPointCloud2
void fromPCLPointCloud2(const pcl::PCLPointCloud2 &msg, pcl::PointCloud< PointT > &cloud, const MsgFieldMap &field_map)
Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
Definition: conversions.h:167
pcl::TextureMapping::sortFacesByCamera
int sortFacesByCamera(pcl::TextureMesh &tex_mesh, pcl::TextureMesh &sorted_mesh, const pcl::texture_mapping::CameraVector &cameras, const double octree_voxel_size, PointCloud &visible_pts)
Segment faces by camera visibility.
Definition: texture_mapping.hpp:532
pcl::texture_mapping::UvIndex::idx_cloud
int idx_cloud
Definition: texture_mapping.h:87