38 #ifndef PCL_SURFACE_IMPL_TEXTURE_MAPPING_HPP_ 39 #define PCL_SURFACE_IMPL_TEXTURE_MAPPING_HPP_ 41 #include <pcl/common/distances.h> 43 #include <pcl/search/octree.h> 44 #include <pcl/common/common.h> 47 template<
typename Po
intInT> std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >
49 const Eigen::Vector3f &p1,
50 const Eigen::Vector3f &p2,
51 const Eigen::Vector3f &p3)
53 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > tex_coordinates;
55 Eigen::Vector3f p1p2 (p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]);
56 Eigen::Vector3f p1p3 (p3[0] - p1[0], p3[1] - p1[1], p3[2] - p1[2]);
57 Eigen::Vector3f p2p3 (p3[0] - p2[0], p3[1] - p2[1], p3[2] - p2[2]);
60 p1p2 = p1p2 /
std::sqrt (p1p2.dot (p1p2));
61 p1p3 = p1p3 /
std::sqrt (p1p3.dot (p1p3));
62 p2p3 = p2p3 /
std::sqrt (p2p3.dot (p2p3));
65 Eigen::Vector3f f_normal = p1p2.cross (p1p3);
66 f_normal = f_normal /
std::sqrt (f_normal.dot (f_normal));
69 Eigen::Vector3f f_vector_field = vector_field_ - vector_field_.dot (f_normal) * f_normal;
72 f_vector_field = f_vector_field /
std::sqrt (f_vector_field.dot (f_vector_field));
75 Eigen::Vector2f tp1, tp2, tp3;
77 double alpha =
std::acos (f_vector_field.dot (p1p2));
80 double e1 = (p2 - p3).norm () / f_;
81 double e2 = (p1 - p3).norm () / f_;
82 double e3 = (p1 - p2).norm () / f_;
88 tp2[0] =
static_cast<float> (e3);
92 double cos_p1 = (e2 * e2 + e3 * e3 - e1 * e1) / (2 * e2 * e3);
93 double sin_p1 =
sqrt (1 - (cos_p1 * cos_p1));
95 tp3[0] =
static_cast<float> (cos_p1 * e2);
96 tp3[1] =
static_cast<float> (sin_p1 * e2);
99 Eigen::Vector2f r_tp2, r_tp3;
100 r_tp2[0] =
static_cast<float> (tp2[0] *
std::cos (alpha) - tp2[1] *
std::sin (alpha));
101 r_tp2[1] =
static_cast<float> (tp2[0] *
std::sin (alpha) + tp2[1] *
std::cos (alpha));
103 r_tp3[0] =
static_cast<float> (tp3[0] *
std::cos (alpha) - tp3[1] *
std::sin (alpha));
104 r_tp3[1] =
static_cast<float> (tp3[0] *
std::sin (alpha) + tp3[1] *
std::cos (alpha));
114 float min_x = tp1[0];
115 float min_y = tp1[1];
127 tp1[0] = tp1[0] - min_x;
128 tp2[0] = tp2[0] - min_x;
129 tp3[0] = tp3[0] - min_x;
133 tp1[1] = tp1[1] - min_y;
134 tp2[1] = tp2[1] - min_y;
135 tp3[1] = tp3[1] - min_y;
138 tex_coordinates.push_back (tp1);
139 tex_coordinates.push_back (tp2);
140 tex_coordinates.push_back (tp3);
141 return (tex_coordinates);
145 template<
typename Po
intInT>
void 149 int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height;
150 int point_size =
static_cast<int> (tex_mesh.cloud.data.size ()) / nr_points;
155 Eigen::Vector3f facet[3];
158 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >texture_map;
160 for (
size_t m = 0; m < tex_mesh.tex_polygons.size (); ++m)
163 #if PCL_VERSION_COMPARE(>=, 1, 8, 0) 164 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
166 std::vector<Eigen::Vector2f> texture_map_tmp;
170 for (
size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i)
175 for (
size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
177 idx = tex_mesh.tex_polygons[m][i].vertices[j];
178 memcpy (&x, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset],
sizeof(
float));
179 memcpy (&y, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset],
sizeof(
float));
180 memcpy (&z, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset],
sizeof(
float));
187 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > tex_coordinates = mapTexture2Face (facet[0], facet[1], facet[2]);
188 for (
size_t n = 0; n < tex_coordinates.size (); ++n)
189 texture_map_tmp.push_back (tex_coordinates[n]);
193 std::stringstream tex_name;
194 tex_name <<
"material_" << m;
195 tex_name >> tex_material_.tex_name;
196 tex_material_.tex_file = tex_files_[m];
197 tex_mesh.tex_materials.push_back (tex_material_);
200 tex_mesh.tex_coordinates.push_back (texture_map_tmp);
205 template<
typename Po
intInT>
void 209 int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height;
210 int point_size =
static_cast<int> (tex_mesh.cloud.data.size ()) / nr_points;
212 float x_lowest = 100000;
214 float y_lowest = 100000;
216 float z_lowest = 100000;
220 for (
int i = 0; i < nr_points; ++i)
222 memcpy (&x_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[0].offset],
sizeof(
float));
223 memcpy (&y_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[1].offset],
sizeof(
float));
224 memcpy (&z_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[2].offset],
sizeof(
float));
243 float x_range = (x_lowest - x_highest) * -1;
244 float x_offset = 0 - x_lowest;
249 float z_range = (z_lowest - z_highest) * -1;
250 float z_offset = 0 - z_lowest;
253 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >texture_map;
255 for (
size_t m = 0; m < tex_mesh.tex_polygons.size (); ++m)
258 #if PCL_VERSION_COMPARE(>=, 1, 8, 0) 259 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
261 std::vector<Eigen::Vector2f> texture_map_tmp;
265 for (
size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i)
268 Eigen::Vector2f tmp_VT;
269 for (
size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
271 idx = tex_mesh.tex_polygons[m][i].vertices[j];
272 memcpy (&x_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset],
sizeof(
float));
273 memcpy (&y_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset],
sizeof(
float));
274 memcpy (&z_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset],
sizeof(
float));
277 tmp_VT[0] = (x_ + x_offset) / x_range;
278 tmp_VT[1] = (z_ + z_offset) / z_range;
279 texture_map_tmp.push_back (tmp_VT);
284 std::stringstream tex_name;
285 tex_name <<
"material_" << m;
286 tex_name >> tex_material_.tex_name;
287 tex_material_.tex_file = tex_files_[m];
288 tex_mesh.tex_materials.push_back (tex_material_);
291 tex_mesh.tex_coordinates.push_back (texture_map_tmp);
296 template<
typename Po
intInT>
void 300 if (tex_mesh.tex_polygons.size () != cams.size () + 1)
302 PCL_ERROR (
"The mesh should be divided into nbCamera+1 sub-meshes.\n");
303 PCL_ERROR (
"You provided %d cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.tex_polygons.size ());
307 PCL_INFO (
"You provided %d cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.tex_polygons.size ());
309 typename pcl::PointCloud<PointInT>::Ptr originalCloud (
new pcl::PointCloud<PointInT>);
310 typename pcl::PointCloud<PointInT>::Ptr camera_transformed_cloud (
new pcl::PointCloud<PointInT>);
313 pcl::fromPCLPointCloud2 (tex_mesh.cloud, *originalCloud);
316 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texture_map;
318 for (
size_t m = 0; m < cams.size (); ++m)
321 Camera current_cam = cams[m];
324 Eigen::Affine3f cam_trans = current_cam.
pose;
330 #if PCL_VERSION_COMPARE(>=, 1, 8, 0) 331 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
333 std::vector<Eigen::Vector2f> texture_map_tmp;
339 for (
size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i)
341 Eigen::Vector2f tmp_VT;
343 for (
size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
346 idx = tex_mesh.tex_polygons[m][i].vertices[j];
347 pt = camera_transformed_cloud->points[idx];
350 getPointUVCoordinates (pt, current_cam, tmp_VT);
351 texture_map_tmp.push_back (tmp_VT);
357 std::stringstream tex_name;
358 tex_name <<
"material_" << m;
359 tex_name >> tex_material_.tex_name;
361 tex_mesh.tex_materials.push_back (tex_material_);
364 tex_mesh.tex_coordinates.push_back (texture_map_tmp);
368 #if PCL_VERSION_COMPARE(>=, 1, 8, 0) 369 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
371 std::vector<Eigen::Vector2f> texture_map_tmp;
373 for (
size_t i = 0; i < tex_mesh.tex_polygons[cams.size ()].size (); ++i)
374 for (
size_t j = 0; j < tex_mesh.tex_polygons[cams.size ()][i].vertices.size (); ++j)
376 Eigen::Vector2f tmp_VT;
379 texture_map_tmp.push_back (tmp_VT);
382 tex_mesh.tex_coordinates.push_back (texture_map_tmp);
385 std::stringstream tex_name;
386 tex_name <<
"material_" << cams.size ();
387 tex_name >> tex_material_.tex_name;
388 tex_material_.tex_file =
"occluded.jpg";
389 tex_mesh.tex_materials.push_back (tex_material_);
394 template<
typename Po
intInT>
bool 397 Eigen::Vector3f direction;
398 direction (0) = pt.x;
399 direction (1) = pt.y;
400 direction (2) = pt.z;
402 std::vector<int> indices;
405 cloud = octree->getInputCloud();
407 double distance_threshold = octree->getResolution();
410 octree->getIntersectedVoxelIndices(direction, -direction, indices);
412 int nbocc =
static_cast<int> (indices.size ());
413 for (
size_t j = 0; j < indices.size (); j++)
416 if (pt.z * cloud->points[indices[j]].z < 0)
422 if (fabs (cloud->points[indices[j]].z - pt.z) <= distance_threshold)
436 template<
typename Po
intInT>
void 439 const double octree_voxel_size, std::vector<int> &visible_indices,
440 std::vector<int> &occluded_indices)
443 double maxDeltaZ = octree_voxel_size;
448 octree->setInputCloud (input_cloud);
450 octree->defineBoundingBox ();
452 octree->addPointsFromInputCloud ();
454 visible_indices.clear ();
457 Eigen::Vector3f direction;
458 std::vector<int> indices;
459 for (
size_t i = 0; i < input_cloud->points.size (); ++i)
461 direction (0) = input_cloud->points[i].x;
462 direction (1) = input_cloud->points[i].y;
463 direction (2) = input_cloud->points[i].z;
466 octree->getIntersectedVoxelIndices (direction, -direction, indices);
468 int nbocc =
static_cast<int> (indices.size ());
469 for (
size_t j = 0; j < indices.size (); j++)
472 if (input_cloud->points[i].z * input_cloud->points[indices[j]].z < 0)
478 if (fabs (input_cloud->points[indices[j]].z - input_cloud->points[i].z) <= maxDeltaZ)
488 filtered_cloud->points.push_back (input_cloud->points[i]);
489 visible_indices.push_back (static_cast<int> (i));
493 occluded_indices.push_back (static_cast<int> (i));
500 template<
typename Po
intInT>
void 504 cleaned_mesh = tex_mesh;
506 typename pcl::PointCloud<PointInT>::Ptr cloud (
new pcl::PointCloud<PointInT>);
507 typename pcl::PointCloud<PointInT>::Ptr filtered_cloud (
new pcl::PointCloud<PointInT>);
510 pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);
512 std::vector<int> visible, occluded;
513 removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded);
517 for (
size_t polygons = 0; polygons < cleaned_mesh.tex_polygons.size (); ++polygons)
520 cleaned_mesh.tex_polygons[polygons].clear ();
522 for (
size_t faces = 0; faces < tex_mesh.tex_polygons[polygons].size (); ++faces)
525 bool faceIsVisible =
true;
526 std::vector<int>::iterator it;
529 for (
size_t points = 0; points < tex_mesh.tex_polygons[polygons][faces].vertices.size (); ++points)
531 it = find (occluded.begin (), occluded.end (), tex_mesh.tex_polygons[polygons][faces].vertices[points]);
533 if (it == occluded.end ())
542 faceIsVisible =
false;
548 cleaned_mesh.tex_polygons[polygons].push_back (tex_mesh.tex_polygons[polygons][faces]);
556 template<
typename Po
intInT>
void 558 const double octree_voxel_size)
563 pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);
565 std::vector<int> visible, occluded;
566 removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded);
571 template<
typename Po
intInT>
int 576 if (tex_mesh.tex_polygons.size () != 1)
578 PCL_ERROR (
"The mesh must contain only 1 sub-mesh!\n");
582 if (cameras.size () == 0)
584 PCL_ERROR (
"Must provide at least one camera info!\n");
589 sorted_mesh = tex_mesh;
591 sorted_mesh.tex_polygons.clear ();
593 typename pcl::PointCloud<PointInT>::Ptr original_cloud (
new pcl::PointCloud<PointInT>);
594 typename pcl::PointCloud<PointInT>::Ptr transformed_cloud (
new pcl::PointCloud<PointInT>);
595 typename pcl::PointCloud<PointInT>::Ptr filtered_cloud (
new pcl::PointCloud<PointInT>);
598 pcl::fromPCLPointCloud2 (tex_mesh.cloud, *original_cloud);
601 for (
size_t cam = 0;
cam < cameras.size (); ++
cam)
604 Eigen::Affine3f cam_trans = cameras[
cam].pose;
610 std::vector<int> visible, occluded;
611 removeOccludedPoints (transformed_cloud, filtered_cloud, octree_voxel_size, visible, occluded);
612 visible_pts = *filtered_cloud;
616 std::vector<pcl::Vertices> visibleFaces_currentCam;
618 for (
size_t faces = 0; faces < tex_mesh.tex_polygons[0].size (); ++faces)
621 bool faceIsVisible =
true;
622 std::vector<int>::iterator it;
625 for (
size_t current_pt_indice = 0; faceIsVisible && current_pt_indice < tex_mesh.tex_polygons[0][faces].vertices.size (); ++current_pt_indice)
628 it = find (occluded.begin (), occluded.end (), tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]);
630 if (it == occluded.end ())
634 PointInT pt = transformed_cloud->points[tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]];
635 Eigen::Vector2f dummy_UV;
636 if (!getPointUVCoordinates (pt, cameras[
cam], dummy_UV))
639 faceIsVisible =
false;
644 faceIsVisible =
false;
651 visibleFaces_currentCam.push_back (tex_mesh.tex_polygons[0][faces]);
653 tex_mesh.tex_polygons[0].erase (tex_mesh.tex_polygons[0].begin () + faces);
658 sorted_mesh.tex_polygons.push_back (visibleFaces_currentCam);
663 sorted_mesh.tex_polygons.push_back (tex_mesh.tex_polygons[0]);
668 template<
typename Po
intInT>
void 670 pcl::PointCloud<pcl::PointXYZI>::Ptr &colored_cloud,
671 const double octree_voxel_size,
const bool show_nb_occlusions,
672 const int max_occlusions)
675 double maxDeltaZ = octree_voxel_size * 2.0;
678 pcl::octree::OctreePointCloudSearch<PointInT> *octree;
679 octree =
new pcl::octree::OctreePointCloudSearch<PointInT> (octree_voxel_size);
681 octree->setInputCloud (input_cloud);
683 octree->defineBoundingBox ();
685 octree->addPointsFromInputCloud ();
688 Eigen::Vector3f direction;
690 std::vector<int> indices;
694 std::vector<double> zDist;
695 std::vector<double> ptDist;
697 for (
size_t i = 0; i < input_cloud->points.size (); ++i)
699 direction (0) = input_cloud->points[i].x;
700 pt.x = input_cloud->points[i].x;
701 direction (1) = input_cloud->points[i].y;
702 pt.y = input_cloud->points[i].y;
703 direction (2) = input_cloud->points[i].z;
704 pt.z = input_cloud->points[i].z;
708 int nbocc = octree->getIntersectedVoxelIndices (direction, -direction, indices);
710 nbocc =
static_cast<int> (indices.size ());
713 for (
size_t j = 0; j < indices.size (); j++)
716 if (pt.z * input_cloud->points[indices[j]].z < 0)
720 else if (fabs (input_cloud->points[indices[j]].z - pt.z) <= maxDeltaZ)
727 zDist.push_back (fabs (input_cloud->points[indices[j]].z - pt.z));
728 ptDist.push_back (pcl::euclideanDistance (input_cloud->points[indices[j]], pt));
732 if (show_nb_occlusions)
733 (nbocc <= max_occlusions) ? (pt.intensity = static_cast<float> (nbocc)) : (pt.intensity = static_cast<float> (max_occlusions));
735 (nbocc == 0) ? (pt.intensity = 0) : (pt.intensity = 1);
737 colored_cloud->points.push_back (pt);
740 if (zDist.size () >= 2)
742 std::sort (zDist.begin (), zDist.end ());
743 std::sort (ptDist.begin (), ptDist.end ());
748 template<
typename Po
intInT>
void 750 double octree_voxel_size,
bool show_nb_occlusions,
int max_occlusions)
753 typename pcl::PointCloud<PointInT>::Ptr cloud (
new pcl::PointCloud<PointInT>);
754 pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);
756 showOcclusions (cloud, colored_cloud, octree_voxel_size, show_nb_occlusions, max_occlusions);
760 template<
typename Po
intInT>
void 764 if (mesh.tex_polygons.size () != 1)
767 typename pcl::PointCloud<PointInT>::Ptr mesh_cloud (
new pcl::PointCloud<PointInT>);
769 pcl::fromPCLPointCloud2 (mesh.cloud, *mesh_cloud);
771 std::vector<pcl::Vertices> faces;
773 for (
int current_cam = 0; current_cam < static_cast<int> (cameras.size ()); ++current_cam)
775 PCL_INFO (
"Processing camera %d of %d.\n", current_cam+1, cameras.size ());
778 typename pcl::PointCloud<PointInT>::Ptr camera_cloud (
new pcl::PointCloud<PointInT>);
782 pcl::PointCloud<pcl::PointXY>::Ptr projections (
new pcl::PointCloud<pcl::PointXY>);
783 std::vector<pcl::Vertices>::iterator current_face;
784 std::vector<bool> visibility;
785 visibility.resize (mesh.tex_polygons[current_cam].size ());
786 std::vector<UvIndex> indexes_uv_to_points;
790 pcl::PointXY nan_point;
791 nan_point.x = std::numeric_limits<float>::quiet_NaN ();
792 nan_point.y = std::numeric_limits<float>::quiet_NaN ();
798 for (
int idx_face = 0; idx_face < static_cast<int> (mesh.tex_polygons[current_cam].size ()); ++idx_face)
801 pcl::PointXY uv_coord1;
802 pcl::PointXY uv_coord2;
803 pcl::PointXY uv_coord3;
805 if (isFaceProjected (cameras[current_cam],
806 camera_cloud->points[mesh.tex_polygons[current_cam][idx_face].vertices[0]],
807 camera_cloud->points[mesh.tex_polygons[current_cam][idx_face].vertices[1]],
808 camera_cloud->points[mesh.tex_polygons[current_cam][idx_face].vertices[2]],
816 projections->points.push_back (uv_coord1);
817 projections->points.push_back (uv_coord2);
818 projections->points.push_back (uv_coord3);
822 u1.
idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[0];
823 u2.
idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[1];
824 u3.
idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[2];
826 indexes_uv_to_points.push_back (u1);
827 indexes_uv_to_points.push_back (u2);
828 indexes_uv_to_points.push_back (u3);
831 visibility[idx_face] =
true;
835 projections->points.push_back (nan_point);
836 projections->points.push_back (nan_point);
837 projections->points.push_back (nan_point);
838 indexes_uv_to_points.push_back (u_null);
839 indexes_uv_to_points.push_back (u_null);
840 indexes_uv_to_points.push_back (u_null);
842 visibility[idx_face] =
false;
852 if (visibility.size () - cpt_invisible !=0)
855 pcl::KdTreeFLANN<pcl::PointXY> kdtree;
856 kdtree.setInputCloud (projections);
858 std::vector<int> idxNeighbors;
859 std::vector<float> neighborsSquaredDistance;
863 for (
int idx_pcam = 0 ; idx_pcam <= current_cam ; ++idx_pcam)
866 for (
int idx_face = 0; idx_face < static_cast<int> (mesh.tex_polygons[idx_pcam].size ()); ++idx_face)
869 if (idx_pcam == current_cam && !visibility[idx_face])
878 pcl::PointXY uv_coord1;
879 pcl::PointXY uv_coord2;
880 pcl::PointXY uv_coord3;
882 if (isFaceProjected (cameras[current_cam],
883 camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[0]],
884 camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[1]],
885 camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[2]],
895 getTriangleCircumcscribedCircleCentroid(uv_coord1, uv_coord2, uv_coord3, center, radius);
898 if (kdtree.radiusSearch (center, radius, idxNeighbors, neighborsSquaredDistance) > 0 )
901 for (
size_t i = 0; i < idxNeighbors.size (); ++i)
903 if (
std::max (camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[0]].z,
904 std::max (camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[1]].z,
905 camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[2]].z))
906 < camera_cloud->points[indexes_uv_to_points[idxNeighbors[i]].idx_cloud].z)
909 if (checkPointInsideTriangle(uv_coord1, uv_coord2, uv_coord3, projections->points[idxNeighbors[i]]))
912 visibility[indexes_uv_to_points[idxNeighbors[i]].idx_face] =
false;
927 if (static_cast<int> (mesh.tex_coordinates.size ()) <= current_cam)
929 #if PCL_VERSION_COMPARE(>=, 1, 8, 0) 930 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > dummy_container;
932 std::vector<Eigen::Vector2f> dummy_container;
934 mesh.tex_coordinates.push_back (dummy_container);
936 mesh.tex_coordinates[current_cam].resize (3 * visibility.size ());
938 std::vector<pcl::Vertices> occluded_faces;
939 occluded_faces.resize (visibility.size ());
940 std::vector<pcl::Vertices> visible_faces;
941 visible_faces.resize (visibility.size ());
943 int cpt_occluded_faces = 0;
944 int cpt_visible_faces = 0;
946 for (
size_t idx_face = 0 ; idx_face < visibility.size () ; ++idx_face)
948 if (visibility[idx_face])
951 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3](0) = projections->points[idx_face*3].x;
952 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3](1) = projections->points[idx_face*3].y;
954 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](0) = projections->points[idx_face*3 + 1].x;
955 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](1) = projections->points[idx_face*3 + 1].y;
957 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](0) = projections->points[idx_face*3 + 2].x;
958 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](1) = projections->points[idx_face*3 + 2].y;
960 visible_faces[cpt_visible_faces] = mesh.tex_polygons[current_cam][idx_face];
967 occluded_faces[cpt_occluded_faces] = mesh.tex_polygons[current_cam][idx_face];
968 cpt_occluded_faces++;
971 mesh.tex_coordinates[current_cam].resize (cpt_visible_faces*3);
973 occluded_faces.resize (cpt_occluded_faces);
974 mesh.tex_polygons.push_back (occluded_faces);
976 visible_faces.resize (cpt_visible_faces);
977 mesh.tex_polygons[current_cam].clear ();
978 mesh.tex_polygons[current_cam] = visible_faces;
981 for (
int i = 0; i < static_cast<int> (mesh.tex_polygons.size ()); i++)
982 nb_faces += static_cast<int> (mesh.tex_polygons[i].size ());
989 if (mesh.tex_coordinates.size() <= cameras.size ())
991 #if PCL_VERSION_COMPARE(>=, 1, 8, 0) 992 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > dummy_container;
994 std::vector<Eigen::Vector2f> dummy_container;
996 mesh.tex_coordinates.push_back(dummy_container);
1000 for(
size_t idx_face = 0 ; idx_face < mesh.tex_polygons[cameras.size()].size() ; ++idx_face)
1002 Eigen::Vector2f UV1, UV2, UV3;
1003 UV1(0) = -1.0; UV1(1) = -1.0;
1004 UV2(0) = -1.0; UV2(1) = -1.0;
1005 UV3(0) = -1.0; UV3(1) = -1.0;
1006 mesh.tex_coordinates[cameras.size()].push_back(UV1);
1007 mesh.tex_coordinates[cameras.size()].push_back(UV2);
1008 mesh.tex_coordinates[cameras.size()].push_back(UV3);
1020 const pcl::PointXY & uv1,
1021 const pcl::PointXY & uv2,
1022 const pcl::PointXY & uv3,
1023 const pcl::PointXY & center) :
1043 bool ptInTriangle(
const pcl::PointXY & p0,
const pcl::PointXY & p1,
const pcl::PointXY & p2,
const pcl::PointXY & p) {
1044 float A = 1/2 * (-p1.y * p2.x + p0.y * (-p1.x + p2.x) + p0.x * (p1.y - p2.y) + p1.x * p2.y);
1045 float sign = A < 0 ? -1 : 1;
1046 float s = (p0.y * p2.x - p0.x * p2.y + (p2.y - p0.y) * p.x + (p0.x - p2.x) * p.y) * sign;
1047 float t = (p0.x * p1.y - p0.y * p1.x + (p0.y - p1.y) * p.x + (p1.x - p0.x) * p.y) * sign;
1049 return s > 0 && t > 0 && (s + t) < 2 * A * sign;
1053 template<
typename Po
intInT>
bool 1055 pcl::TextureMesh &mesh,
1058 std::vector<std::map<int, pcl::PointXY> > * vertexToPixels,
1059 bool distanceToCamPolicy)
1062 if (mesh.tex_polygons.size () != 1)
1065 typename pcl::PointCloud<PointInT>::Ptr mesh_cloud (
new pcl::PointCloud<PointInT>);
1067 pcl::fromPCLPointCloud2 (mesh.cloud, *mesh_cloud);
1069 std::vector<pcl::Vertices> faces;
1070 faces.swap(mesh.tex_polygons[0]);
1072 mesh.tex_polygons.clear();
1073 mesh.tex_polygons.resize(cameras.size()+1);
1074 mesh.tex_coordinates.clear();
1075 mesh.tex_coordinates.resize(cameras.size()+1);
1078 std::vector<std::map<int, FaceInfo > > visibleFaces(cameras.size());
1079 std::vector<Eigen::Affine3f> invCamTransform(cameras.size());
1080 std::vector<std::list<int> > faceCameras(faces.size());
1081 std::string msg =
uFormat(
"Computing visible faces per cam (%d faces, %d cams)", (
int)faces.size(), (int)cameras.size());
1086 UWARN(
"Texturing cancelled!");
1089 for (
unsigned int current_cam = 0; current_cam < cameras.size(); ++current_cam)
1091 UDEBUG(
"Texture camera %d...", current_cam);
1093 typename pcl::PointCloud<PointInT>::Ptr camera_cloud (
new pcl::PointCloud<PointInT>);
1096 std::vector<int> visibilityIndices;
1097 visibilityIndices.resize (faces.size ());
1098 pcl::PointCloud<pcl::PointXY>::Ptr projections (
new pcl::PointCloud<pcl::PointXY>);
1099 projections->resize(faces.size()*3);
1100 std::map<float, int> sortedVisibleFaces;
1102 for(
unsigned int idx_face=0; idx_face<faces.size(); ++idx_face)
1104 pcl::Vertices & face = faces[idx_face];
1107 pcl::PointXY & uv_coords1 = projections->at(j);
1108 pcl::PointXY & uv_coords2 = projections->at(j+1);
1109 pcl::PointXY & uv_coords3 = projections->at(j+2);
1110 PointInT & pt0 = camera_cloud->points[face.vertices[0]];
1111 PointInT & pt1 = camera_cloud->points[face.vertices[1]];
1112 PointInT & pt2 = camera_cloud->points[face.vertices[2]];
1113 if (isFaceProjected (cameras[current_cam],
1123 uv_coords2.x - uv_coords1.x,
1124 uv_coords2.y - uv_coords1.y,
1127 uv_coords3.x - uv_coords1.x,
1128 uv_coords3.y - uv_coords1.y,
1130 Eigen::Vector3f normal = v0.cross(v1);
1131 float angle = normal.dot(Eigen::Vector3f(0.0
f,0.0
f,1.0
f));
1134 float angleToCam = 0.0f;
1135 Eigen::Vector3f e0 = Eigen::Vector3f(
1139 Eigen::Vector3f e1 = Eigen::Vector3f(
1143 Eigen::Vector3f e2 = Eigen::Vector3f(
1147 if(facingTheCam && this->max_angle_)
1149 Eigen::Vector3f normal3D;
1150 normal3D = e0.cross(e1);
1151 angleToCam = pcl::getAngle3D(Eigen::Vector4f(normal3D[0], normal3D[1], normal3D[2], 0.0
f), Eigen::Vector4f(0.0
f,0.0
f,-1.0
f,0.0
f));
1155 float e0norm2 = e0[0]*e0[0] + e0[1]*e0[1] + e0[2]*e0[2];
1156 float e1norm2 = e1[0]*e1[0] + e1[1]*e1[1] + e1[2]*e1[2];
1157 float e2norm2 = e2[0]*e2[0] + e2[1]*e2[1] + e2[2]*e2[2];
1160 pcl::PointXY center;
1161 center.x = (uv_coords1.x+uv_coords2.x+uv_coords3.x)/3.0
f;
1162 center.y = (uv_coords1.y+uv_coords2.y+uv_coords3.y)/3.0
f;
1163 visibleFaces[current_cam].insert(visibleFaces[current_cam].end(), std::make_pair(idx_face,
FaceInfo(distanceToCam, angleToCam, longestEdgeSqrd, facingTheCam, uv_coords1, uv_coords2, uv_coords3, center)));
1164 sortedVisibleFaces.insert(std::make_pair(distanceToCam, idx_face));
1165 visibilityIndices[oi] = idx_face;
1169 visibilityIndices.resize(oi);
1170 projections->resize(oi*3);
1171 UASSERT(projections->size() == visibilityIndices.size()*3);
1175 pcl::KdTreeFLANN<pcl::PointXY> kdtree;
1176 kdtree.setInputCloud (projections);
1178 std::vector<int> idxNeighbors;
1179 std::vector<float> neighborsSquaredDistance;
1183 std::set<int> occludedFaces;
1184 for (std::map<float, int>::iterator jter=sortedVisibleFaces.begin(); jter!=sortedVisibleFaces.end(); ++jter)
1187 int idx_face = jter->second;
1189 std::map<int, FaceInfo>::iterator iter= visibleFaces[current_cam].find(idx_face);
1190 UASSERT(iter != visibleFaces[current_cam].end());
1197 pcl::PointXY center;
1202 if (kdtree.radiusSearch (center, radius, idxNeighbors, neighborsSquaredDistance) > 0 )
1205 for (
size_t i = 0; i < idxNeighbors.size (); ++i)
1207 int neighborFaceIndex = idxNeighbors[i]/3;
1211 if (
std::max(camera_cloud->points[faces[idx_face].vertices[0]].z,
1212 std::max (camera_cloud->points[faces[idx_face].vertices[1]].z,
1213 camera_cloud->points[faces[idx_face].vertices[2]].z))
1214 < camera_cloud->points[faces[visibilityIndices[neighborFaceIndex]].vertices[idxNeighbors[i]%3]].z)
1221 occludedFaces.insert(visibilityIndices[neighborFaceIndex]);
1231 for(std::set<int>::iterator iter= occludedFaces.begin(); iter!=occludedFaces.end(); ++iter)
1233 visibleFaces[current_cam].erase(*iter);
1237 int clusterFaces = 0;
1239 std::vector<pcl::Vertices> polygons(visibleFaces[current_cam].size());
1240 std::vector<int> polygon_to_face_index(visibleFaces[current_cam].size());
1242 for(std::map<int, FaceInfo>::iterator iter=visibleFaces[current_cam].begin(); iter!=visibleFaces[current_cam].end(); ++iter)
1244 polygons[oi].vertices.resize(3);
1245 polygons[oi].vertices[0] = faces[iter->first].vertices[0];
1246 polygons[oi].vertices[1] = faces[iter->first].vertices[1];
1247 polygons[oi].vertices[2] = faces[iter->first].vertices[2];
1248 polygon_to_face_index[oi] = iter->first;
1252 std::vector<std::set<int> > neighbors;
1253 std::vector<std::set<int> > vertexToPolygons;
1255 (
int)camera_cloud->size(),
1261 std::set<int> polygonsKept;
1262 for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
1264 for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
1266 polygonsKept.insert(polygon_to_face_index[*jter]);
1267 faceCameras[polygon_to_face_index[*jter]].push_back(current_cam);
1271 for(std::map<int, FaceInfo>::iterator iter=visibleFaces[current_cam].begin(); iter!=visibleFaces[current_cam].end();)
1273 if(polygonsKept.find(iter->first) == polygonsKept.end())
1275 visibleFaces[current_cam].erase(iter++);
1284 msg =
uFormat(
"Processed camera %d/%d: %d occluded and %d spurious polygons out of %d", (
int)current_cam+1, (
int)cameras.size(), (int)occludedFaces.size(), clusterFaces, (int)visibilityIndices.size());
1289 UWARN(
"Texturing cancelled!");
1294 msg =
uFormat(
"Texturing %d polygons...", (
int)faces.size());
1299 UWARN(
"Texturing cancelled!");
1305 *vertexToPixels = std::vector<std::map<int, pcl::PointXY> >(mesh_cloud->size());
1307 for(
unsigned int idx_face=0; idx_face<faces.size(); ++idx_face)
1309 if((idx_face+1)%10000 == 0)
1311 UDEBUG(
"face %d/%d", idx_face+1, (
int)faces.size());
1312 if(state && !state->
callback(
uFormat(
"Textured %d/%d of %d polygons", textured, idx_face+1, (
int)faces.size())))
1315 UWARN(
"Texturing cancelled!");
1319 pcl::Vertices & face = faces[idx_face];
1321 int cameraIndex = -1;
1323 bool depthSet =
false;
1324 pcl::PointXY uv_coords[3];
1325 for (std::list<int>::iterator camIter = faceCameras[idx_face].begin(); camIter!=faceCameras[idx_face].end(); ++camIter)
1327 int current_cam = *camIter;
1328 std::map<int, FaceInfo>::iterator iter = visibleFaces[current_cam].find(idx_face);
1329 UASSERT(iter != visibleFaces[current_cam].end());
1330 if (iter->second.facingTheCam && (max_angle_ <=0.0f || iter->second.angle <= max_angle_))
1332 float distanceToCam = iter->second.distance;
1333 float vx = (iter->second.uv_coord1.x+iter->second.uv_coord2.x+ iter->second.uv_coord3.x)/3.0
f-0.5
f;
1334 float vy = (iter->second.uv_coord1.y+iter->second.uv_coord2.y+ iter->second.uv_coord3.y)/3.0
f-0.5
f;
1335 float distanceToCenter = vx*vx+vy*vy;
1337 cv::Mat depth = cameras[current_cam].depth;
1338 bool currentDepthSet =
false;
1339 float maxDepthError = max_depth_error_==0.0f?
std::sqrt(iter->second.longestEdgeSqrd)*2.0f : max_depth_error_;
1340 if(!cameras[current_cam].depth.empty() && maxDepthError > 0.0f)
1342 float d1 = depth.type() == CV_32FC1?
1343 depth.at<
float>((1.0f-iter->second.uv_coord1.y)*depth.rows, iter->second.uv_coord1.x*depth.cols):
1344 float(depth.at<
unsigned short>((1.0f-iter->second.uv_coord1.y)*depth.rows, iter->second.uv_coord1.x*depth.cols))/1000.0f;
1345 float d2 = depth.type() == CV_32FC1?
1346 depth.at<
float>((1.0f-iter->second.uv_coord2.y)*depth.rows, iter->second.uv_coord2.x*depth.cols):
1347 float(depth.at<
unsigned short>((1.0f-iter->second.uv_coord2.y)*depth.rows, iter->second.uv_coord2.x*depth.cols))/1000.0f;
1348 float d3 = depth.type() == CV_32FC1?
1349 depth.at<
float>((1.0f-iter->second.uv_coord3.y)*depth.rows, iter->second.uv_coord3.x*depth.cols):
1350 float(depth.at<
unsigned short>((1.0f-iter->second.uv_coord3.y)*depth.rows, iter->second.uv_coord3.x*depth.cols))/1000.0f;
1358 else if(d1 > 0.0
f &&
std::isfinite(d1) && fabs(d1 - distanceToCam) > maxDepthError)
1363 else if(d2 > 0.0
f &&
std::isfinite(d2) && fabs(d2 - distanceToCam) > maxDepthError)
1368 else if(d3 > 0.0
f &&
std::isfinite(d3) && fabs(d3 - distanceToCam) > maxDepthError)
1377 if(fabs(d1 - distanceToCam) > maxDepthError ||
1378 fabs(d2 - distanceToCam) > maxDepthError ||
1379 fabs(d3 - distanceToCam) > maxDepthError)
1384 currentDepthSet =
true;
1390 vertexToPixels->at(face.vertices[0]).insert(std::make_pair(current_cam, iter->second.uv_coord1));
1391 vertexToPixels->at(face.vertices[1]).insert(std::make_pair(current_cam, iter->second.uv_coord2));
1392 vertexToPixels->at(face.vertices[2]).insert(std::make_pair(current_cam, iter->second.uv_coord3));
1398 if(distanceToCamPolicy)
1400 distance = distanceToCam;
1402 if(distance <= smallestWeight || (!depthSet && currentDepthSet))
1404 cameraIndex = current_cam;
1406 uv_coords[0] = iter->second.uv_coord1;
1407 uv_coords[1] = iter->second.uv_coord2;
1408 uv_coords[2] = iter->second.uv_coord3;
1409 if(!depthSet && currentDepthSet)
1417 if(cameraIndex >= 0)
1420 mesh.tex_polygons[cameraIndex].push_back(face);
1421 mesh.tex_coordinates[cameraIndex].push_back(Eigen::Vector2f(uv_coords[0].x, uv_coords[0].y));
1422 mesh.tex_coordinates[cameraIndex].push_back(Eigen::Vector2f(uv_coords[1].x, uv_coords[1].y));
1423 mesh.tex_coordinates[cameraIndex].push_back(Eigen::Vector2f(uv_coords[2].x, uv_coords[2].y));
1427 mesh.tex_polygons[cameras.size()].push_back(face);
1428 mesh.tex_coordinates[cameras.size()].push_back(Eigen::Vector2f(-1.0,-1.0));
1429 mesh.tex_coordinates[cameras.size()].push_back(Eigen::Vector2f(-1.0,-1.0));
1430 mesh.tex_coordinates[cameras.size()].push_back(Eigen::Vector2f(-1.0,-1.0));
1433 UINFO(
"Process %d polygons...done! (%d textured)", (
int)faces.size(), textured);
1439 template<
typename Po
intInT>
inline void 1443 pcl::PointXY ptB, ptC;
1444 ptB.x = p2.x - p1.x; ptB.y = p2.y - p1.y;
1445 ptC.x = p3.x - p1.x; ptC.y = p3.y - p1.y;
1447 double D = 2.0*(ptB.x*ptC.y - ptB.y*ptC.x);
1452 circomcenter.x = p1.x;
1453 circomcenter.y = p1.y;
1458 double bx2 = ptB.x * ptB.x;
1459 double by2 = ptB.y * ptB.y;
1460 double cx2 = ptC.x * ptC.x;
1461 double cy2 = ptC.y * ptC.y;
1464 circomcenter.x =
static_cast<float> (p1.x + (ptC.y*(bx2 + by2) - ptB.y*(cx2 + cy2)) / D);
1465 circomcenter.y =
static_cast<float> (p1.y + (ptB.x*(cx2 + cy2) - ptC.x*(bx2 + by2)) / D);
1468 radius =
sqrt( (circomcenter.x - p1.x)*(circomcenter.x - p1.x) + (circomcenter.y - p1.y)*(circomcenter.y - p1.y));
1472 template<
typename Po
intInT>
inline void 1476 circumcenter.x =
static_cast<float> (p1.x + p2.x + p3.x ) / 3;
1477 circumcenter.y =
static_cast<float> (p1.y + p2.y + p3.y ) / 3;
1478 double r1 = (circumcenter.x - p1.x) * (circumcenter.x - p1.x) + (circumcenter.y - p1.y) * (circumcenter.y - p1.y) ;
1479 double r2 = (circumcenter.x - p2.x) * (circumcenter.x - p2.x) + (circumcenter.y - p2.y) * (circumcenter.y - p2.y) ;
1480 double r3 = (circumcenter.x - p3.x) * (circumcenter.x - p3.x) + (circumcenter.y - p3.y) * (circumcenter.y - p3.y) ;
1488 template<
typename Po
intInT>
inline bool 1491 if (pt.z > 0 && (max_distance_<=0.0f || pt.z<max_distance_))
1494 double sizeX = cam.
width;
1495 double sizeY = cam.
height;
1506 double focal_x, focal_y;
1517 UV_coordinates.x =
static_cast<float> ((focal_x * (pt.x / pt.z) + cx) / sizeX);
1518 UV_coordinates.y =
static_cast<float> ((focal_y * (pt.y / pt.z) + cy) / sizeY);
1520 if(cam.
roi.size() == 4)
1522 if( UV_coordinates.x < cam.
roi[0]/sizeX ||
1523 UV_coordinates.y < cam.
roi[1]/sizeY ||
1524 UV_coordinates.x > (cam.
roi[0]+cam.
roi[2])/sizeX ||
1525 UV_coordinates.y > (cam.
roi[1]+cam.
roi[3])/sizeY)
1528 UV_coordinates.x = -1.0f;
1529 UV_coordinates.y = -1.0f;
1535 if (UV_coordinates.x >= 0.0 && UV_coordinates.x <= 1.0 && UV_coordinates.y >= 0.0 && UV_coordinates.y <= 1.0)
1539 UV_coordinates.y = 1.0f - UV_coordinates.y;
1545 UV_coordinates.x = -1.0f;
1546 UV_coordinates.y = -1.0f;
1551 template<
typename Po
intInT>
inline bool 1555 Eigen::Vector2d v0, v1, v2;
1556 v0(0) = p3.x - p1.x; v0(1) = p3.y - p1.y;
1557 v1(0) = p2.x - p1.x; v1(1) = p2.y - p1.y;
1558 v2(0) = pt.x - p1.x; v2(1) = pt.y - p1.y;
1561 double dot00 = v0.dot(v0);
1562 double dot01 = v0.dot(v1);
1563 double dot02 = v0.dot(v2);
1564 double dot11 = v1.dot(v1);
1565 double dot12 = v1.dot(v2);
1568 double invDenom = 1.0 / (dot00*dot11 - dot01*dot01);
1569 double u = (dot11*dot02 - dot01*dot12) * invDenom;
1570 double v = (dot00*dot12 - dot01*dot02) * invDenom;
1573 return ((u >= 0) && (v >= 0) && (u + v < 1));
1577 template<
typename Po
intInT>
inline bool 1580 return getPointUVCoordinates(p1, camera, proj1)
1582 getPointUVCoordinates(p2, camera, proj2)
1584 getPointUVCoordinates(p3, camera, proj3);
1587 #define PCL_INSTANTIATE_TextureMapping(T) \ 1588 template class PCL_EXPORTS pcl::TextureMapping<T>;
rtabmap::CameraThread * cam
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. Point-based segmentation.
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.
GLM_FUNC_DECL bool isfinite(genType const &x)
Test whether or not a scalar or each vector component is a finite value. (From GLM_GTX_compatibility)...
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
bool textureMeshwithMultipleCameras2(pcl::TextureMesh &mesh, const pcl::texture_mapping::CameraVector &cameras, const rtabmap::ProgressState *callback=0, std::vector< std::map< int, pcl::PointXY > > *vertexToPixels=0, bool distanceToCamPolicy=false)
std::vector< Camera, Eigen::aligned_allocator< Camera > > CameraVector
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
const float maxDepthError
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.
FaceInfo(float d, float a, float edge, bool facingCam, const pcl::PointXY &uv1, const pcl::PointXY &uv2, const pcl::PointXY &uv3, const pcl::PointXY ¢er)
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.
pcl::octree::OctreePointCloudSearch< PointInT > Octree
GLM_FUNC_DECL genType sign(genType const &x)
std::vector< double > roi
#define UASSERT(condition)
GLM_FUNC_DECL genType cos(genType const &angle)
bool ptInTriangle(const pcl::PointXY &p0, const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p)
Structure that links a uv coordinate to its 3D point and face.
Structure to store camera pose and focal length.
virtual bool callback(const std::string &msg) const
GLM_FUNC_DECL genType sin(genType const &angle)
std::list< std::list< int > > RTABMAP_EXP clusterPolygons(const std::vector< std::set< int > > &neighborPolygons, int minClusterSize=0)
pcl::PointCloud< PointInT > PointCloud
void mapTexture2MeshUV(pcl::TextureMesh &tex_mesh)
Map texture to a mesh UV mapping.
PointCloud::Ptr PointCloudPtr
void textureMeshwithMultipleCameras(pcl::TextureMesh &mesh, const pcl::texture_mapping::CameraVector &cameras)
Segment and texture faces by camera visibility. Face-based segmentation.
void mapMultipleTexturesToMeshUV(pcl::TextureMesh &tex_mesh, pcl::texture_mapping::CameraVector &cams)
Map textures acquired from a set of cameras onto a mesh.
void mapTexture2Mesh(pcl::TextureMesh &tex_mesh)
Map texture to a mesh synthesis algorithm.
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.
RecoveryProgressState state
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.
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.
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
void removeOccludedPoints(const PointCloudPtr &input_cloud, PointCloudPtr &filtered_cloud, const double octree_voxel_size, std::vector< int > &visible_indices, std::vector< int > &occluded_indices)
Remove occluded points from a point cloud.
void RTABMAP_EXP createPolygonIndexes(const std::vector< pcl::Vertices > &polygons, int cloudSize, std::vector< std::set< int > > &neighborPolygons, std::vector< std::set< int > > &vertexPolygons)
Given a set of polygons, create two indexes: polygons to neighbor polygons and vertices to polygons...
GLM_FUNC_DECL genType acos(genType const &x)
bool getPointUVCoordinates(const PointInT &pt, const Camera &cam, Eigen::Vector2f &UV_coordinates)
computes UV coordinates of point, observed by one particular camera
PointCloud::ConstPtr PointCloudConstPtr
std::string UTILITE_EXP uFormat(const char *fmt,...)