Point Cloud Library (PCL) 1.12.1
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
42#include <pcl/surface/texture_mapping.h>
43#include <unordered_set>
44
45///////////////////////////////////////////////////////////////////////////////////////////////
46template<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///////////////////////////////////////////////////////////////////////////////////////////////
144template<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 // get facet information
168 for (std::size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
169 {
170 std::size_t idx = tex_mesh.tex_polygons[m][i].vertices[j];
171 memcpy (&x, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float));
172 memcpy (&y, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float));
173 memcpy (&z, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float));
174 facet[j][0] = x;
175 facet[j][1] = y;
176 facet[j][2] = z;
177 }
178
179 // get texture coordinates of each face
180 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > tex_coordinates = mapTexture2Face (facet[0], facet[1], facet[2]);
181 for (const auto &tex_coordinate : tex_coordinates)
182 texture_map_tmp.push_back (tex_coordinate);
183 }// end faces
184
185 // texture materials
186 tex_material_.tex_name = "material_" + std::to_string(m);
187 tex_material_.tex_file = tex_files_[m];
188 tex_mesh.tex_materials.push_back (tex_material_);
189
190 // texture coordinates
191 tex_mesh.tex_coordinates.push_back (texture_map_tmp);
192 }// end meshes
193}
194
195///////////////////////////////////////////////////////////////////////////////////////////////
196template<typename PointInT> void
198{
199 // mesh information
200 int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height;
201 int point_size = static_cast<int> (tex_mesh.cloud.data.size ()) / nr_points;
202
203 float x_lowest = 100000;
204 float x_highest = 0;
205 float y_lowest = 100000;
206 //float y_highest = 0 ;
207 float z_lowest = 100000;
208 float z_highest = 0;
209 float x_, y_, z_;
210
211 for (int i = 0; i < nr_points; ++i)
212 {
213 memcpy (&x_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float));
214 memcpy (&y_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float));
215 memcpy (&z_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float));
216 // x
217 if (x_ <= x_lowest)
218 x_lowest = x_;
219 if (x_ > x_lowest)
220 x_highest = x_;
221
222 // y
223 if (y_ <= y_lowest)
224 y_lowest = y_;
225 //if (y_ > y_lowest) y_highest = y_;
226
227 // z
228 if (z_ <= z_lowest)
229 z_lowest = z_;
230 if (z_ > z_lowest)
231 z_highest = z_;
232 }
233 // x
234 float x_range = (x_lowest - x_highest) * -1;
235 float x_offset = 0 - x_lowest;
236 // x
237 // float y_range = (y_lowest - y_highest)*-1;
238 // float y_offset = 0 - y_lowest;
239 // z
240 float z_range = (z_lowest - z_highest) * -1;
241 float z_offset = 0 - z_lowest;
242
243 // texture coordinates for each mesh
244 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >texture_map;
245
246 for (std::size_t m = 0; m < tex_mesh.tex_polygons.size (); ++m)
247 {
248 // texture coordinates for each mesh
249 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
250
251 // processing for each face
252 for (std::size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i)
253 {
254 Eigen::Vector2f tmp_VT;
255 for (std::size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
256 {
257 std::size_t idx = tex_mesh.tex_polygons[m][i].vertices[j];
258 memcpy (&x_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float));
259 memcpy (&y_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float));
260 memcpy (&z_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float));
261
262 // calculate uv coordinates
263 tmp_VT[0] = (x_ + x_offset) / x_range;
264 tmp_VT[1] = (z_ + z_offset) / z_range;
265 texture_map_tmp.push_back (tmp_VT);
266 }
267 }// end faces
268
269 // texture materials
270 tex_material_.tex_name = "material_" + std::to_string(m);
271 tex_material_.tex_file = tex_files_[m];
272 tex_mesh.tex_materials.push_back (tex_material_);
273
274 // texture coordinates
275 tex_mesh.tex_coordinates.push_back (texture_map_tmp);
276 }// end meshes
277}
278
279///////////////////////////////////////////////////////////////////////////////////////////////
280template<typename PointInT> void
282{
283
284 if (tex_mesh.tex_polygons.size () != cams.size () + 1)
285 {
286 PCL_ERROR ("The mesh should be divided into nbCamera+1 sub-meshes.\n");
287 PCL_ERROR ("You provided %d cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.tex_polygons.size ());
288 return;
289 }
290
291 PCL_INFO ("You provided %d cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.tex_polygons.size ());
292
294 typename pcl::PointCloud<PointInT>::Ptr camera_transformed_cloud (new pcl::PointCloud<PointInT>);
295
296 // convert mesh's cloud to pcl format for ease
297 pcl::fromPCLPointCloud2 (tex_mesh.cloud, *originalCloud);
298
299 for (std::size_t m = 0; m < cams.size (); ++m)
300 {
301 // get current camera parameters
302 Camera current_cam = cams[m];
303
304 // get camera transform
305 Eigen::Affine3f cam_trans = current_cam.pose;
306
307 // transform cloud into current camera frame
308 pcl::transformPointCloud (*originalCloud, *camera_transformed_cloud, cam_trans.inverse ());
309
310 // vector of texture coordinates for each face
311 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
312
313 // processing each face visible by this camera
314 for (const auto &tex_polygon : tex_mesh.tex_polygons[m])
315 {
316 Eigen::Vector2f tmp_VT;
317 // for each point of this face
318 for (const auto &vertex : tex_polygon.vertices)
319 {
320 // get point
321 PointInT pt = (*camera_transformed_cloud)[vertex];
322
323 // compute UV coordinates for this point
324 getPointUVCoordinates (pt, current_cam, tmp_VT);
325 texture_map_tmp.push_back (tmp_VT);
326 }// end points
327 }// end faces
328
329 // texture materials
330 tex_material_.tex_name = "material_" + std::to_string(m);
331 tex_material_.tex_file = current_cam.texture_file;
332 tex_mesh.tex_materials.push_back (tex_material_);
333
334 // texture coordinates
335 tex_mesh.tex_coordinates.push_back (texture_map_tmp);
336 }// end cameras
337
338 // push on extra empty UV map (for unseen faces) so that obj writer does not crash!
339 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
340 for (const auto &tex_polygon : tex_mesh.tex_polygons[cams.size ()])
341 for (std::size_t j = 0; j < tex_polygon.vertices.size (); ++j)
342 {
343 Eigen::Vector2f tmp_VT;
344 tmp_VT[0] = -1;
345 tmp_VT[1] = -1;
346 texture_map_tmp.push_back (tmp_VT);
347 }
348
349 tex_mesh.tex_coordinates.push_back (texture_map_tmp);
350
351 // push on an extra dummy material for the same reason
352 tex_material_.tex_name = "material_" + std::to_string(cams.size());
353 tex_material_.tex_file = "occluded.jpg";
354 tex_mesh.tex_materials.push_back (tex_material_);
355}
356
357///////////////////////////////////////////////////////////////////////////////////////////////
358template<typename PointInT> bool
360{
361 Eigen::Vector3f direction;
362 direction (0) = pt.x;
363 direction (1) = pt.y;
364 direction (2) = pt.z;
365
366 pcl::Indices indices;
367
368 PointCloudConstPtr cloud (new PointCloud());
369 cloud = octree->getInputCloud();
370
371 double distance_threshold = octree->getResolution();
372
373 // raytrace
374 octree->getIntersectedVoxelIndices(direction, -direction, indices);
375
376 int nbocc = static_cast<int> (indices.size ());
377 for (const auto &index : indices)
378 {
379 // if intersected point is on the over side of the camera
380 if (pt.z * (*cloud)[index].z < 0)
381 {
382 nbocc--;
383 continue;
384 }
385
386 if (std::fabs ((*cloud)[index].z - pt.z) <= distance_threshold)
387 {
388 // points are very close to each-other, we do not consider the occlusion
389 nbocc--;
390 }
391 }
392
393 return (nbocc != 0);
394}
395
396///////////////////////////////////////////////////////////////////////////////////////////////
397template<typename PointInT> void
399 PointCloudPtr &filtered_cloud,
400 const double octree_voxel_size, pcl::Indices &visible_indices,
401 pcl::Indices &occluded_indices)
402{
403 // variable used to filter occluded points by depth
404 double maxDeltaZ = octree_voxel_size;
405
406 // create an octree to perform rayTracing
407 Octree octree (octree_voxel_size);
408 // create octree structure
409 octree.setInputCloud (input_cloud);
410 // update bounding box automatically
411 octree.defineBoundingBox ();
412 // add points in the tree
413 octree.addPointsFromInputCloud ();
414
415 visible_indices.clear ();
416
417 // for each point of the cloud, raycast toward camera and check intersected voxels.
418 Eigen::Vector3f direction;
419 pcl::Indices indices;
420 for (std::size_t i = 0; i < input_cloud->size (); ++i)
421 {
422 direction (0) = (*input_cloud)[i].x;
423 direction (1) = (*input_cloud)[i].y;
424 direction (2) = (*input_cloud)[i].z;
425
426 // if point is not occluded
427 octree.getIntersectedVoxelIndices (direction, -direction, indices);
428
429 int nbocc = static_cast<int> (indices.size ());
430 for (const auto &index : indices)
431 {
432 // if intersected point is on the over side of the camera
433 if ((*input_cloud)[i].z * (*input_cloud)[index].z < 0)
434 {
435 nbocc--;
436 continue;
437 }
438
439 if (std::fabs ((*input_cloud)[index].z - (*input_cloud)[i].z) <= maxDeltaZ)
440 {
441 // points are very close to each-other, we do not consider the occlusion
442 nbocc--;
443 }
444 }
445
446 if (nbocc == 0)
447 {
448 // point is added in the filtered mesh
449 filtered_cloud->points.push_back ((*input_cloud)[i]);
450 visible_indices.push_back (static_cast<pcl::index_t> (i));
451 }
452 else
453 {
454 occluded_indices.push_back (static_cast<pcl::index_t> (i));
455 }
456 }
457
458}
459
460///////////////////////////////////////////////////////////////////////////////////////////////
461template<typename PointInT> void
462pcl::TextureMapping<PointInT>::removeOccludedPoints (const pcl::TextureMesh &tex_mesh, pcl::TextureMesh &cleaned_mesh, const double octree_voxel_size)
463{
464 // copy mesh
465 cleaned_mesh = tex_mesh;
466
468 typename pcl::PointCloud<PointInT>::Ptr filtered_cloud (new pcl::PointCloud<PointInT>);
469
470 // load points into a PCL format
471 pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);
472
473 pcl::Indices visible, occluded;
474 removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded);
475
476 // Now that we know which points are visible, let's iterate over each face.
477 // if the face has one invisible point => out!
478 for (std::size_t polygons = 0; polygons < cleaned_mesh.tex_polygons.size (); ++polygons)
479 {
480 // remove all faces from cleaned mesh
481 cleaned_mesh.tex_polygons[polygons].clear ();
482 // iterate over faces
483 for (std::size_t faces = 0; faces < tex_mesh.tex_polygons[polygons].size (); ++faces)
484 {
485 // check if all the face's points are visible
486 bool faceIsVisible = true;
487
488 // iterate over face's vertex
489 for (const auto &vertex : tex_mesh.tex_polygons[polygons][faces].vertices)
490 {
491 if (find (occluded.begin (), occluded.end (), vertex) == occluded.end ())
492 {
493 // point is not in the occluded vector
494 // PCL_INFO (" VISIBLE!\n");
495 }
496 else
497 {
498 // point was occluded
499 // PCL_INFO(" OCCLUDED!\n");
500 faceIsVisible = false;
501 }
502 }
503
504 if (faceIsVisible)
505 {
506 cleaned_mesh.tex_polygons[polygons].push_back (tex_mesh.tex_polygons[polygons][faces]);
507 }
508
509 }
510 }
511}
512
513///////////////////////////////////////////////////////////////////////////////////////////////
514template<typename PointInT> void
516 const double octree_voxel_size)
517{
518 PointCloudPtr cloud (new PointCloud);
519
520 // load points into a PCL format
521 pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);
522
523 pcl::Indices visible, occluded;
524 removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded);
525
526}
527
528///////////////////////////////////////////////////////////////////////////////////////////////
529template<typename PointInT> int
531 const pcl::texture_mapping::CameraVector &cameras, const double octree_voxel_size,
532 PointCloud &visible_pts)
533{
534 if (tex_mesh.tex_polygons.size () != 1)
535 {
536 PCL_ERROR ("The mesh must contain only 1 sub-mesh!\n");
537 return (-1);
538 }
539
540 if (cameras.empty ())
541 {
542 PCL_ERROR ("Must provide at least one camera info!\n");
543 return (-1);
544 }
545
546 // copy mesh
547 sorted_mesh = tex_mesh;
548 // clear polygons from cleaned_mesh
549 sorted_mesh.tex_polygons.clear ();
550
551 typename pcl::PointCloud<PointInT>::Ptr original_cloud (new pcl::PointCloud<PointInT>);
552 typename pcl::PointCloud<PointInT>::Ptr transformed_cloud (new pcl::PointCloud<PointInT>);
553 typename pcl::PointCloud<PointInT>::Ptr filtered_cloud (new pcl::PointCloud<PointInT>);
554
555 // load points into a PCL format
556 pcl::fromPCLPointCloud2 (tex_mesh.cloud, *original_cloud);
557
558 // for each camera
559 for (const auto &camera : cameras)
560 {
561 // get camera pose as transform
562 Eigen::Affine3f cam_trans = camera.pose;
563
564 // transform original cloud in camera coordinates
565 pcl::transformPointCloud (*original_cloud, *transformed_cloud, cam_trans.inverse ());
566
567 // find occlusions on transformed cloud
568 pcl::Indices visible, occluded;
569 removeOccludedPoints (transformed_cloud, filtered_cloud, octree_voxel_size, visible, occluded);
570 visible_pts = *filtered_cloud;
571
572 // pushing occluded idxs into a set for faster lookup
573 std::unordered_set<index_t> occluded_set(occluded.cbegin(), occluded.cend());
574
575 // find visible faces => add them to polygon N for camera N
576 // add polygon group for current camera in clean
577 std::vector<pcl::Vertices> visibleFaces_currentCam;
578 // iterate over the faces of the current mesh
579 for (std::size_t faces = 0; faces < tex_mesh.tex_polygons[0].size (); ++faces)
580 {
581 // check if all the face's points are visible
582 // iterate over face's vertex
583 const auto faceIsVisible = std::all_of(tex_mesh.tex_polygons[0][faces].vertices.cbegin(),
584 tex_mesh.tex_polygons[0][faces].vertices.cend(),
585 [&](const auto& vertex)
586 {
587 if (occluded_set.find(vertex) != occluded_set.cend()) {
588 return false; // point is occluded
589 }
590 // is the point visible to the camera?
591 Eigen::Vector2f dummy_UV;
592 return this->getPointUVCoordinates ((*transformed_cloud)[vertex], camera, dummy_UV);
593 });
594
595 if (faceIsVisible)
596 {
597 // push current visible face into the sorted mesh
598 visibleFaces_currentCam.push_back (tex_mesh.tex_polygons[0][faces]);
599 // remove it from the unsorted mesh
600 tex_mesh.tex_polygons[0].erase (tex_mesh.tex_polygons[0].begin () + faces);
601 faces--;
602 }
603
604 }
605 sorted_mesh.tex_polygons.push_back (visibleFaces_currentCam);
606 }
607
608 // we should only have occluded and non-visible faces left in tex_mesh.tex_polygons[0]
609 // we need to add them as an extra polygon in the sorted mesh
610 sorted_mesh.tex_polygons.push_back (tex_mesh.tex_polygons[0]);
611 return (0);
612}
613
614///////////////////////////////////////////////////////////////////////////////////////////////
615template<typename PointInT> void
618 const double octree_voxel_size, const bool show_nb_occlusions,
619 const int max_occlusions)
620 {
621 // variable used to filter occluded points by depth
622 double maxDeltaZ = octree_voxel_size * 2.0;
623
624 // create an octree to perform rayTracing
625 Octree octree (octree_voxel_size);
626 // create octree structure
627 octree.setInputCloud (input_cloud);
628 // update bounding box automatically
629 octree.defineBoundingBox ();
630 // add points in the tree
631 octree.addPointsFromInputCloud ();
632
633 // ray direction
634 Eigen::Vector3f direction;
635
636 pcl::Indices indices;
637 // point from where we ray-trace
639
640 std::vector<double> zDist;
641 std::vector<double> ptDist;
642 // for each point of the cloud, ray-trace toward the camera and check intersected voxels.
643 for (const auto& point: *input_cloud)
644 {
645 direction = pt.getVector3fMap() = point.getVector3fMap();
646
647 // get number of occlusions for that point
648 indices.clear ();
649 int nbocc = octree.getIntersectedVoxelIndices (direction, -direction, indices);
650
651 nbocc = static_cast<int> (indices.size ());
652
653 // TODO need to clean this up and find tricks to get remove aliasaing effect on planes
654 for (const auto &index : indices)
655 {
656 // if intersected point is on the over side of the camera
657 if (pt.z * (*input_cloud)[index].z < 0)
658 {
659 nbocc--;
660 }
661 else if (std::fabs ((*input_cloud)[index].z - pt.z) <= maxDeltaZ)
662 {
663 // points are very close to each-other, we do not consider the occlusion
664 nbocc--;
665 }
666 else
667 {
668 zDist.push_back (std::fabs ((*input_cloud)[index].z - pt.z));
669 ptDist.push_back (pcl::euclideanDistance ((*input_cloud)[index], pt));
670 }
671 }
672
673 if (show_nb_occlusions)
674 (nbocc <= max_occlusions) ? (pt.intensity = static_cast<float> (nbocc)) : (pt.intensity = static_cast<float> (max_occlusions));
675 else
676 (nbocc == 0) ? (pt.intensity = 0) : (pt.intensity = 1);
677
678 colored_cloud->points.push_back (pt);
679 }
680
681 if (zDist.size () >= 2)
682 {
683 std::sort (zDist.begin (), zDist.end ());
684 std::sort (ptDist.begin (), ptDist.end ());
685 }
686}
687
688///////////////////////////////////////////////////////////////////////////////////////////////
689template<typename PointInT> void
691 double octree_voxel_size, bool show_nb_occlusions, int max_occlusions)
692{
693 // load points into a PCL format
695 pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);
696
697 showOcclusions (cloud, colored_cloud, octree_voxel_size, show_nb_occlusions, max_occlusions);
698}
699
700///////////////////////////////////////////////////////////////////////////////////////////////
701template<typename PointInT> void
703{
704
705 if (mesh.tex_polygons.size () != 1)
706 return;
707
709
710 pcl::fromPCLPointCloud2 (mesh.cloud, *mesh_cloud);
711
712 std::vector<pcl::Vertices> faces;
713
714 for (int current_cam = 0; current_cam < static_cast<int> (cameras.size ()); ++current_cam)
715 {
716 PCL_INFO ("Processing camera %d of %d.\n", current_cam+1, cameras.size ());
717
718 // transform mesh into camera's frame
720 pcl::transformPointCloud (*mesh_cloud, *camera_cloud, cameras[current_cam].pose.inverse ());
721
722 // CREATE UV MAP FOR CURRENT FACES
724 std::vector<bool> visibility;
725 visibility.resize (mesh.tex_polygons[current_cam].size ());
726 std::vector<UvIndex> indexes_uv_to_points;
727 // for each current face
728
729 //TODO change this
730 pcl::PointXY nan_point;
731 nan_point.x = std::numeric_limits<float>::quiet_NaN ();
732 nan_point.y = std::numeric_limits<float>::quiet_NaN ();
733 UvIndex u_null;
734 u_null.idx_cloud = -1;
735 u_null.idx_face = -1;
736
737 int cpt_invisible=0;
738 for (int idx_face = 0; idx_face < static_cast<int> (mesh.tex_polygons[current_cam].size ()); ++idx_face)
739 {
740 //project each vertice, if one is out of view, stop
741 pcl::PointXY uv_coord1;
742 pcl::PointXY uv_coord2;
743 pcl::PointXY uv_coord3;
744
745 if (isFaceProjected (cameras[current_cam],
746 (*camera_cloud)[mesh.tex_polygons[current_cam][idx_face].vertices[0]],
747 (*camera_cloud)[mesh.tex_polygons[current_cam][idx_face].vertices[1]],
748 (*camera_cloud)[mesh.tex_polygons[current_cam][idx_face].vertices[2]],
749 uv_coord1,
750 uv_coord2,
751 uv_coord3))
752 {
753 // face is in the camera's FOV
754
755 // add UV coordinates
756 projections->points.push_back (uv_coord1);
757 projections->points.push_back (uv_coord2);
758 projections->points.push_back (uv_coord3);
759
760 // remember corresponding face
761 UvIndex u1, u2, u3;
762 u1.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[0];
763 u2.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[1];
764 u3.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[2];
765 u1.idx_face = idx_face; u2.idx_face = idx_face; u3.idx_face = idx_face;
766 indexes_uv_to_points.push_back (u1);
767 indexes_uv_to_points.push_back (u2);
768 indexes_uv_to_points.push_back (u3);
769
770 //keep track of visibility
771 visibility[idx_face] = true;
772 }
773 else
774 {
775 projections->points.push_back (nan_point);
776 projections->points.push_back (nan_point);
777 projections->points.push_back (nan_point);
778 indexes_uv_to_points.push_back (u_null);
779 indexes_uv_to_points.push_back (u_null);
780 indexes_uv_to_points.push_back (u_null);
781 //keep track of visibility
782 visibility[idx_face] = false;
783 cpt_invisible++;
784 }
785 }
786
787 // projections contains all UV points of the current faces
788 // indexes_uv_to_points links a uv point to its point in the camera cloud
789 // visibility contains tells if a face was in the camera FOV (false = skip)
790
791 // TODO handle case were no face could be projected
792 if (visibility.size () - cpt_invisible !=0)
793 {
794 //create kdtree
796 kdtree.setInputCloud (projections);
797
798 pcl::Indices idxNeighbors;
799 std::vector<float> neighborsSquaredDistance;
800 // af first (idx_pcan < current_cam), check if some of the faces attached to previous cameras occlude the current faces
801 // then (idx_pcam == current_cam), check for self occlusions. At this stage, we skip faces that were already marked as occluded
802 cpt_invisible = 0;
803 for (int idx_pcam = 0 ; idx_pcam <= current_cam ; ++idx_pcam)
804 {
805 // project all faces
806 for (int idx_face = 0; idx_face < static_cast<int> (mesh.tex_polygons[idx_pcam].size ()); ++idx_face)
807 {
808
809 if (idx_pcam == current_cam && !visibility[idx_face])
810 {
811 // we are now checking for self occlusions within the current faces
812 // the current face was already declared as occluded.
813 // therefore, it cannot occlude another face anymore => we skip it
814 continue;
815 }
816
817 // project each vertice, if one is out of view, stop
818 pcl::PointXY uv_coord1;
819 pcl::PointXY uv_coord2;
820 pcl::PointXY uv_coord3;
821
822 if (isFaceProjected (cameras[current_cam],
823 (*camera_cloud)[mesh.tex_polygons[idx_pcam][idx_face].vertices[0]],
824 (*camera_cloud)[mesh.tex_polygons[idx_pcam][idx_face].vertices[1]],
825 (*camera_cloud)[mesh.tex_polygons[idx_pcam][idx_face].vertices[2]],
826 uv_coord1,
827 uv_coord2,
828 uv_coord3))
829 {
830 // face is in the camera's FOV
831 //get its circumsribed circle
832 double radius;
833 pcl::PointXY center;
834 // getTriangleCircumcenterAndSize (uv_coord1, uv_coord2, uv_coord3, center, radius);
835 getTriangleCircumcscribedCircleCentroid(uv_coord1, uv_coord2, uv_coord3, center, radius); // this function yields faster results than getTriangleCircumcenterAndSize
836
837 // get points inside circ.circle
838 if (kdtree.radiusSearch (center, radius, idxNeighbors, neighborsSquaredDistance) > 0 )
839 {
840 // for each neighbor
841 for (const auto &idxNeighbor : idxNeighbors)
842 {
843 if (std::max ((*camera_cloud)[mesh.tex_polygons[idx_pcam][idx_face].vertices[0]].z,
844 std::max ((*camera_cloud)[mesh.tex_polygons[idx_pcam][idx_face].vertices[1]].z,
845 (*camera_cloud)[mesh.tex_polygons[idx_pcam][idx_face].vertices[2]].z))
846 < (*camera_cloud)[indexes_uv_to_points[idxNeighbor].idx_cloud].z)
847 {
848 // neighbor is farther than all the face's points. Check if it falls into the triangle
849 if (checkPointInsideTriangle(uv_coord1, uv_coord2, uv_coord3, (*projections)[idxNeighbor]))
850 {
851 // current neighbor is inside triangle and is closer => the corresponding face
852 visibility[indexes_uv_to_points[idxNeighbor].idx_face] = false;
853 cpt_invisible++;
854 //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
855 }
856 }
857 }
858 }
859 }
860 }
861 }
862 }
863
864 // now, visibility is true for each face that belongs to the current camera
865 // if a face is not visible, we push it into the next one.
866
867 if (static_cast<int> (mesh.tex_coordinates.size ()) <= current_cam)
868 {
869 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > dummy_container;
870 mesh.tex_coordinates.push_back (dummy_container);
871 }
872 mesh.tex_coordinates[current_cam].resize (3 * visibility.size ());
873
874 std::vector<pcl::Vertices> occluded_faces;
875 occluded_faces.resize (visibility.size ());
876 std::vector<pcl::Vertices> visible_faces;
877 visible_faces.resize (visibility.size ());
878
879 int cpt_occluded_faces = 0;
880 int cpt_visible_faces = 0;
881
882 for (std::size_t idx_face = 0 ; idx_face < visibility.size () ; ++idx_face)
883 {
884 if (visibility[idx_face])
885 {
886 // face is visible by the current camera copy UV coordinates
887 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3](0) = (*projections)[idx_face*3].x;
888 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3](1) = (*projections)[idx_face*3].y;
889
890 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](0) = (*projections)[idx_face*3 + 1].x;
891 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](1) = (*projections)[idx_face*3 + 1].y;
892
893 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](0) = (*projections)[idx_face*3 + 2].x;
894 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](1) = (*projections)[idx_face*3 + 2].y;
895
896 visible_faces[cpt_visible_faces] = mesh.tex_polygons[current_cam][idx_face];
897
898 cpt_visible_faces++;
899 }
900 else
901 {
902 // face is occluded copy face into temp vector
903 occluded_faces[cpt_occluded_faces] = mesh.tex_polygons[current_cam][idx_face];
904 cpt_occluded_faces++;
905 }
906 }
907 mesh.tex_coordinates[current_cam].resize (cpt_visible_faces*3);
908
909 occluded_faces.resize (cpt_occluded_faces);
910 mesh.tex_polygons.push_back (occluded_faces);
911
912 visible_faces.resize (cpt_visible_faces);
913 mesh.tex_polygons[current_cam].clear ();
914 mesh.tex_polygons[current_cam] = visible_faces;
915 }
916
917 // we have been through all the cameras.
918 // if any faces are left, they were not visible by any camera
919 // we still need to produce uv coordinates for them
920
921 if (mesh.tex_coordinates.size() <= cameras.size ())
922 {
923 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > dummy_container;
924 mesh.tex_coordinates.push_back(dummy_container);
925 }
926
927
928 for(std::size_t idx_face = 0 ; idx_face < mesh.tex_polygons[cameras.size()].size() ; ++idx_face)
929 {
930 Eigen::Vector2f UV1, UV2, UV3;
931 UV1(0) = -1.0; UV1(1) = -1.0;
932 UV2(0) = -1.0; UV2(1) = -1.0;
933 UV3(0) = -1.0; UV3(1) = -1.0;
934 mesh.tex_coordinates[cameras.size()].push_back(UV1);
935 mesh.tex_coordinates[cameras.size()].push_back(UV2);
936 mesh.tex_coordinates[cameras.size()].push_back(UV3);
937 }
938
939}
940
941///////////////////////////////////////////////////////////////////////////////////////////////
942template<typename PointInT> inline void
944{
945 // we simplify the problem by translating the triangle's origin to its first point
946 pcl::PointXY ptB, ptC;
947 ptB.x = p2.x - p1.x; ptB.y = p2.y - p1.y; // B'=B-A
948 ptC.x = p3.x - p1.x; ptC.y = p3.y - p1.y; // C'=C-A
949
950 double D = 2.0*(ptB.x*ptC.y - ptB.y*ptC.x); // D'=2(B'x*C'y - B'y*C'x)
951
952 // Safety check to avoid division by zero
953 if(D == 0)
954 {
955 circomcenter.x = p1.x;
956 circomcenter.y = p1.y;
957 }
958 else
959 {
960 // compute squares once
961 double bx2 = ptB.x * ptB.x; // B'x^2
962 double by2 = ptB.y * ptB.y; // B'y^2
963 double cx2 = ptC.x * ptC.x; // C'x^2
964 double cy2 = ptC.y * ptC.y; // C'y^2
965
966 // compute circomcenter's coordinates (translate back to original coordinates)
967 circomcenter.x = static_cast<float> (p1.x + (ptC.y*(bx2 + by2) - ptB.y*(cx2 + cy2)) / D);
968 circomcenter.y = static_cast<float> (p1.y + (ptB.x*(cx2 + cy2) - ptC.x*(bx2 + by2)) / D);
969 }
970
971 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));
972}
973
974///////////////////////////////////////////////////////////////////////////////////////////////
975template<typename PointInT> inline void
977{
978 // compute centroid's coordinates (translate back to original coordinates)
979 circumcenter.x = static_cast<float> (p1.x + p2.x + p3.x ) / 3;
980 circumcenter.y = static_cast<float> (p1.y + p2.y + p3.y ) / 3;
981 double r1 = (circumcenter.x - p1.x) * (circumcenter.x - p1.x) + (circumcenter.y - p1.y) * (circumcenter.y - p1.y) ;
982 double r2 = (circumcenter.x - p2.x) * (circumcenter.x - p2.x) + (circumcenter.y - p2.y) * (circumcenter.y - p2.y) ;
983 double r3 = (circumcenter.x - p3.x) * (circumcenter.x - p3.x) + (circumcenter.y - p3.y) * (circumcenter.y - p3.y) ;
984
985 // radius
986 radius = std::sqrt( std::max( r1, std::max( r2, r3) )) ;
987}
988
989
990///////////////////////////////////////////////////////////////////////////////////////////////
991template<typename PointInT> inline bool
992pcl::TextureMapping<PointInT>::getPointUVCoordinates(const PointInT &pt, const Camera &cam, pcl::PointXY &UV_coordinates)
993{
994 if (pt.z > 0)
995 {
996 // compute image center and dimension
997 double sizeX = cam.width;
998 double sizeY = cam.height;
999 double cx, cy;
1000 if (cam.center_w > 0)
1001 cx = cam.center_w;
1002 else
1003 cx = sizeX / 2.0;
1004 if (cam.center_h > 0)
1005 cy = cam.center_h;
1006 else
1007 cy = sizeY / 2.0;
1008
1009 double focal_x, focal_y;
1010 if (cam.focal_length_w > 0)
1011 focal_x = cam.focal_length_w;
1012 else
1013 focal_x = cam.focal_length;
1014 if (cam.focal_length_h > 0)
1015 focal_y = cam.focal_length_h;
1016 else
1017 focal_y = cam.focal_length;
1018
1019 // project point on camera's image plane
1020 UV_coordinates.x = static_cast<float> ((focal_x * (pt.x / pt.z) + cx) / sizeX); //horizontal
1021 UV_coordinates.y = 1.0f - static_cast<float> ((focal_y * (pt.y / pt.z) + cy) / sizeY); //vertical
1022
1023 // point is visible!
1024 if (UV_coordinates.x >= 0.0 && UV_coordinates.x <= 1.0 && UV_coordinates.y >= 0.0 && UV_coordinates.y <= 1.0)
1025 return (true); // point was visible by the camera
1026 }
1027
1028 // point is NOT visible by the camera
1029 UV_coordinates.x = -1.0f;
1030 UV_coordinates.y = -1.0f;
1031 return (false); // point was not visible by the camera
1032}
1033
1034///////////////////////////////////////////////////////////////////////////////////////////////
1035template<typename PointInT> inline bool
1037{
1038 // Compute vectors
1039 Eigen::Vector2d v0, v1, v2;
1040 v0(0) = p3.x - p1.x; v0(1) = p3.y - p1.y; // v0= C - A
1041 v1(0) = p2.x - p1.x; v1(1) = p2.y - p1.y; // v1= B - A
1042 v2(0) = pt.x - p1.x; v2(1) = pt.y - p1.y; // v2= P - A
1043
1044 // Compute dot products
1045 double dot00 = v0.dot(v0); // dot00 = dot(v0, v0)
1046 double dot01 = v0.dot(v1); // dot01 = dot(v0, v1)
1047 double dot02 = v0.dot(v2); // dot02 = dot(v0, v2)
1048 double dot11 = v1.dot(v1); // dot11 = dot(v1, v1)
1049 double dot12 = v1.dot(v2); // dot12 = dot(v1, v2)
1050
1051 // Compute barycentric coordinates
1052 double invDenom = 1.0 / (dot00*dot11 - dot01*dot01);
1053 double u = (dot11*dot02 - dot01*dot12) * invDenom;
1054 double v = (dot00*dot12 - dot01*dot02) * invDenom;
1055
1056 // Check if point is in triangle
1057 return ((u >= 0) && (v >= 0) && (u + v < 1));
1058}
1059
1060///////////////////////////////////////////////////////////////////////////////////////////////
1061template<typename PointInT> inline bool
1062pcl::TextureMapping<PointInT>::isFaceProjected (const Camera &camera, const PointInT &p1, const PointInT &p2, const PointInT &p3, pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3)
1063{
1064 return (getPointUVCoordinates(p1, camera, proj1)
1065 &&
1066 getPointUVCoordinates(p2, camera, proj2)
1067 &&
1068 getPointUVCoordinates(p3, camera, proj3)
1069 );
1070}
1071
1072#define PCL_INSTANTIATE_TextureMapping(T) \
1073 template class PCL_EXPORTS pcl::TextureMapping<T>;
1074
1075#endif /* TEXTURE_MAPPING_HPP_ */
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
Definition: kdtree_flann.h:132
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.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:395
bool getPointUVCoordinates(const PointInT &pt, const Camera &cam, Eigen::Vector2f &UV_coordinates)
computes UV coordinates of point, observed by one particular camera
void mapTexture2MeshUV(pcl::TextureMesh &tex_mesh)
Map texture to a mesh UV mapping.
void mapTexture2Mesh(pcl::TextureMesh &tex_mesh)
Map texture to a mesh synthesis algorithm.
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.
typename PointCloud::Ptr PointCloudPtr
typename Octree::Ptr OctreePtr
bool isPointOccluded(const PointInT &pt, const OctreePtr octree)
Check if a point is occluded using raycasting on octree.
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.
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.
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.
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.
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.
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.
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.
typename PointCloud::ConstPtr PointCloudConstPtr
void mapMultipleTexturesToMeshUV(pcl::TextureMesh &tex_mesh, pcl::texture_mapping::CameraVector &cams)
Map textures acquired from a set of cameras onto a mesh.
void textureMeshwithMultipleCameras(pcl::TextureMesh &mesh, const pcl::texture_mapping::CameraVector &cameras)
Segment and texture faces by camera visibility.
void defineBoundingBox()
Investigate dimensions of pointcloud data set and define corresponding bounding box for octree.
void setInputCloud(const PointCloudConstPtr &cloud_arg, const IndicesConstPtr &indices_arg=IndicesConstPtr())
Provide a pointer to the input data set.
void addPointsFromInputCloud()
Add points from input point cloud to octree.
Octree pointcloud search class
Definition: octree_search.h:58
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).
Define standard C methods to do distance calculations.
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
std::vector< Camera, Eigen::aligned_allocator< Camera > > CameraVector
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
float euclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the euclidean distance between the two given points.
Definition: distances.h:204
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
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:166
std::vector<::pcl::PCLPointField > fields
std::vector< std::uint8_t > data
A 2D point structure representing Euclidean xy coordinates.
std::vector< std::vector< pcl::Vertices > > tex_polygons
Definition: TextureMesh.h:94
std::vector< std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > > tex_coordinates
Definition: TextureMesh.h:95
std::vector< pcl::TexMaterial > tex_materials
Definition: TextureMesh.h:96
pcl::PCLPointCloud2 cloud
Definition: TextureMesh.h:90
Structure to store camera pose and focal length.
Structure that links a uv coordinate to its 3D point and face.