38 #ifndef PCL_SURFACE_IMPL_TEXTURE_MAPPING_HPP_
39 #define PCL_SURFACE_IMPL_TEXTURE_MAPPING_HPP_
42 #include <pcl/surface/texture_mapping.h>
43 #include <unordered_set>
46 template<
typename Po
intInT> std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >
48 const Eigen::Vector3f &p1,
49 const Eigen::Vector3f &p2,
50 const Eigen::Vector3f &p3)
52 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > tex_coordinates;
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]);
59 p1p2 /= std::sqrt (p1p2.dot (p1p2));
60 p1p3 /= std::sqrt (p1p3.dot (p1p3));
61 p2p3 /= std::sqrt (p2p3.dot (p2p3));
64 Eigen::Vector3f f_normal = p1p2.cross (p1p3);
65 f_normal /= std::sqrt (f_normal.dot (f_normal));
68 Eigen::Vector3f f_vector_field = vector_field_ - vector_field_.dot (f_normal) * f_normal;
71 f_vector_field /= std::sqrt (f_vector_field.dot (f_vector_field));
74 Eigen::Vector2f tp1, tp2, tp3;
76 double alpha = std::acos (f_vector_field.dot (p1p2));
79 double e1 = (p2 - p3).norm () / f_;
80 double e2 = (p1 - p3).norm () / f_;
81 double e3 = (p1 - p2).norm () / f_;
87 tp2[0] =
static_cast<float> (e3);
91 double cos_p1 = (e2 * e2 + e3 * e3 - e1 * e1) / (2 * e2 * e3);
92 double sin_p1 = sqrt (1 - (cos_p1 * cos_p1));
94 tp3[0] =
static_cast<float> (cos_p1 * e2);
95 tp3[1] =
static_cast<float> (sin_p1 * e2);
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));
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));
113 float min_x = tp1[0];
114 float min_y = tp1[1];
137 tex_coordinates.push_back (tp1);
138 tex_coordinates.push_back (tp2);
139 tex_coordinates.push_back (tp3);
140 return (tex_coordinates);
144 template<
typename Po
intInT>
void
149 int point_size =
static_cast<int> (tex_mesh.
cloud.
data.size ()) / nr_points;
154 Eigen::Vector3f facet[3];
157 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >texture_map;
159 for (std::size_t m = 0; m < tex_mesh.
tex_polygons.size (); ++m)
162 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
165 for (std::size_t i = 0; i < tex_mesh.
tex_polygons[m].size (); ++i)
170 for (std::size_t j = 0; j < tex_mesh.
tex_polygons[m][i].vertices.size (); ++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));
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);
188 std::stringstream tex_name;
189 tex_name <<
"material_" << m;
190 tex_name >> tex_material_.tex_name;
191 tex_material_.tex_file = tex_files_[m];
200 template<
typename Po
intInT>
void
205 int point_size =
static_cast<int> (tex_mesh.
cloud.
data.size ()) / nr_points;
207 float x_lowest = 100000;
209 float y_lowest = 100000;
211 float z_lowest = 100000;
215 for (
int i = 0; i < nr_points; ++i)
217 memcpy (&x_, &tex_mesh.
cloud.
data[i * point_size + tex_mesh.
cloud.
fields[0].offset],
sizeof(
float));
218 memcpy (&y_, &tex_mesh.
cloud.
data[i * point_size + tex_mesh.
cloud.
fields[1].offset],
sizeof(
float));
219 memcpy (&z_, &tex_mesh.
cloud.
data[i * point_size + tex_mesh.
cloud.
fields[2].offset],
sizeof(
float));
238 float x_range = (x_lowest - x_highest) * -1;
239 float x_offset = 0 - x_lowest;
244 float z_range = (z_lowest - z_highest) * -1;
245 float z_offset = 0 - z_lowest;
248 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >texture_map;
250 for (std::size_t m = 0; m < tex_mesh.
tex_polygons.size (); ++m)
253 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
256 for (std::size_t i = 0; i < tex_mesh.
tex_polygons[m].size (); ++i)
258 Eigen::Vector2f tmp_VT;
259 for (std::size_t j = 0; j < tex_mesh.
tex_polygons[m][i].vertices.size (); ++j)
261 std::size_t idx = tex_mesh.
tex_polygons[m][i].vertices[j];
262 memcpy (&x_, &tex_mesh.
cloud.
data[idx * point_size + tex_mesh.
cloud.
fields[0].offset],
sizeof(
float));
263 memcpy (&y_, &tex_mesh.
cloud.
data[idx * point_size + tex_mesh.
cloud.
fields[1].offset],
sizeof(
float));
264 memcpy (&z_, &tex_mesh.
cloud.
data[idx * point_size + tex_mesh.
cloud.
fields[2].offset],
sizeof(
float));
267 tmp_VT[0] = (x_ + x_offset) / x_range;
268 tmp_VT[1] = (z_ + z_offset) / z_range;
269 texture_map_tmp.push_back (tmp_VT);
274 std::stringstream tex_name;
275 tex_name <<
"material_" << m;
276 tex_name >> tex_material_.tex_name;
277 tex_material_.tex_file = tex_files_[m];
286 template<
typename Po
intInT>
void
292 PCL_ERROR (
"The mesh should be divided into nbCamera+1 sub-meshes.\n");
293 PCL_ERROR (
"You provided %d cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.
tex_polygons.size ());
297 PCL_INFO (
"You provided %d cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.
tex_polygons.size ());
305 for (std::size_t m = 0; m < cams.size (); ++m)
308 Camera current_cam = cams[m];
311 Eigen::Affine3f cam_trans = current_cam.
pose;
317 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
320 for (
const auto &tex_polygon : tex_mesh.
tex_polygons[m])
322 Eigen::Vector2f tmp_VT;
324 for (
const auto &vertex : tex_polygon.vertices)
327 PointInT pt = (*camera_transformed_cloud)[vertex];
330 getPointUVCoordinates (pt, current_cam, tmp_VT);
331 texture_map_tmp.push_back (tmp_VT);
336 std::stringstream tex_name;
337 tex_name <<
"material_" << m;
338 tex_name >> tex_material_.tex_name;
347 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
348 for (
const auto &tex_polygon : tex_mesh.
tex_polygons[cams.size ()])
349 for (std::size_t j = 0; j < tex_polygon.vertices.size (); ++j)
351 Eigen::Vector2f tmp_VT;
354 texture_map_tmp.push_back (tmp_VT);
360 tex_material_.tex_name =
"material_" + std::to_string(cams.size());
361 tex_material_.tex_file =
"occluded.jpg";
366 template<
typename Po
intInT>
bool
369 Eigen::Vector3f direction;
370 direction (0) = pt.x;
371 direction (1) = pt.y;
372 direction (2) = pt.z;
377 cloud = octree->getInputCloud();
379 double distance_threshold = octree->getResolution();
382 octree->getIntersectedVoxelIndices(direction, -direction, indices);
384 int nbocc =
static_cast<int> (indices.size ());
385 for (
const auto &index : indices)
388 if (pt.z * (*cloud)[index].z < 0)
394 if (std::fabs ((*cloud)[index].z - pt.z) <= distance_threshold)
405 template<
typename Po
intInT>
void
408 const double octree_voxel_size,
pcl::Indices &visible_indices,
412 double maxDeltaZ = octree_voxel_size;
415 Octree octree (octree_voxel_size);
423 visible_indices.clear ();
426 Eigen::Vector3f direction;
428 for (std::size_t i = 0; i < input_cloud->size (); ++i)
430 direction (0) = (*input_cloud)[i].x;
431 direction (1) = (*input_cloud)[i].y;
432 direction (2) = (*input_cloud)[i].z;
437 int nbocc =
static_cast<int> (indices.size ());
438 for (
const auto &index : indices)
441 if ((*input_cloud)[i].z * (*input_cloud)[index].z < 0)
447 if (std::fabs ((*input_cloud)[index].z - (*input_cloud)[i].z) <= maxDeltaZ)
457 filtered_cloud->points.push_back ((*input_cloud)[i]);
458 visible_indices.push_back (
static_cast<pcl::index_t> (i));
462 occluded_indices.push_back (
static_cast<pcl::index_t> (i));
469 template<
typename Po
intInT>
void
473 cleaned_mesh = tex_mesh;
482 removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded);
486 for (std::size_t polygons = 0; polygons < cleaned_mesh.
tex_polygons.size (); ++polygons)
491 for (std::size_t faces = 0; faces < tex_mesh.
tex_polygons[polygons].size (); ++faces)
494 bool faceIsVisible =
true;
497 for (
const auto &vertex : tex_mesh.
tex_polygons[polygons][faces].vertices)
499 if (find (occluded.begin (), occluded.end (), vertex) == occluded.end ())
508 faceIsVisible =
false;
522 template<
typename Po
intInT>
void
524 const double octree_voxel_size)
532 removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded);
537 template<
typename Po
intInT>
int
544 PCL_ERROR (
"The mesh must contain only 1 sub-mesh!\n");
548 if (cameras.empty ())
550 PCL_ERROR (
"Must provide at least one camera info!\n");
555 sorted_mesh = tex_mesh;
567 for (
const auto &camera : cameras)
570 Eigen::Affine3f cam_trans = camera.pose;
577 removeOccludedPoints (transformed_cloud, filtered_cloud, octree_voxel_size, visible, occluded);
578 visible_pts = *filtered_cloud;
581 std::unordered_set<index_t> occluded_set(occluded.cbegin(), occluded.cend());
585 std::vector<pcl::Vertices> visibleFaces_currentCam;
587 for (std::size_t faces = 0; faces < tex_mesh.
tex_polygons[0].size (); ++faces)
591 const auto faceIsVisible = std::all_of(tex_mesh.
tex_polygons[0][faces].vertices.cbegin(),
593 [&](
const auto& vertex)
595 if (occluded_set.find(vertex) != occluded_set.cend()) {
599 Eigen::Vector2f dummy_UV;
600 return this->getPointUVCoordinates ((*transformed_cloud)[vertex], camera, dummy_UV);
606 visibleFaces_currentCam.push_back (tex_mesh.
tex_polygons[0][faces]);
613 sorted_mesh.
tex_polygons.push_back (visibleFaces_currentCam);
618 sorted_mesh.tex_polygons.push_back (tex_mesh.tex_polygons[0]);
623 template<
typename Po
intInT>
void
626 const double octree_voxel_size,
const bool show_nb_occlusions,
627 const int max_occlusions)
630 double maxDeltaZ = octree_voxel_size * 2.0;
633 Octree octree (octree_voxel_size);
642 Eigen::Vector3f direction;
648 std::vector<double> zDist;
649 std::vector<double> ptDist;
651 for (
const auto& point: *input_cloud)
653 direction = pt.getVector3fMap() = point.getVector3fMap();
659 nbocc =
static_cast<int> (indices.size ());
662 for (
const auto &index : indices)
665 if (pt.z * (*input_cloud)[index].z < 0)
669 else if (std::fabs ((*input_cloud)[index].z - pt.z) <= maxDeltaZ)
676 zDist.push_back (std::fabs ((*input_cloud)[index].z - pt.z));
681 if (show_nb_occlusions)
682 (nbocc <= max_occlusions) ? (pt.
intensity =
static_cast<float> (nbocc)) : (pt.
intensity =
static_cast<float> (max_occlusions));
686 colored_cloud->
points.push_back (pt);
689 if (zDist.size () >= 2)
691 std::sort (zDist.begin (), zDist.end ());
692 std::sort (ptDist.begin (), ptDist.end ());
697 template<
typename Po
intInT>
void
699 double octree_voxel_size,
bool show_nb_occlusions,
int max_occlusions)
705 showOcclusions (cloud, colored_cloud, octree_voxel_size, show_nb_occlusions, max_occlusions);
709 template<
typename Po
intInT>
void
720 std::vector<pcl::Vertices> faces;
722 for (
int current_cam = 0; current_cam < static_cast<int> (cameras.size ()); ++current_cam)
724 PCL_INFO (
"Processing camera %d of %d.\n", current_cam+1, cameras.size ());
732 std::vector<bool> visibility;
733 visibility.resize (mesh.
tex_polygons[current_cam].size ());
734 std::vector<UvIndex> indexes_uv_to_points;
739 nan_point.
x = std::numeric_limits<float>::quiet_NaN ();
740 nan_point.
y = std::numeric_limits<float>::quiet_NaN ();
746 for (
int idx_face = 0; idx_face < static_cast<int> (mesh.
tex_polygons[current_cam].size ()); ++idx_face)
753 if (isFaceProjected (cameras[current_cam],
754 (*camera_cloud)[mesh.
tex_polygons[current_cam][idx_face].vertices[0]],
755 (*camera_cloud)[mesh.
tex_polygons[current_cam][idx_face].vertices[1]],
756 (*camera_cloud)[mesh.
tex_polygons[current_cam][idx_face].vertices[2]],
764 projections->
points.push_back (uv_coord1);
765 projections->
points.push_back (uv_coord2);
766 projections->
points.push_back (uv_coord3);
774 indexes_uv_to_points.push_back (u1);
775 indexes_uv_to_points.push_back (u2);
776 indexes_uv_to_points.push_back (u3);
779 visibility[idx_face] =
true;
783 projections->
points.push_back (nan_point);
784 projections->
points.push_back (nan_point);
785 projections->
points.push_back (nan_point);
786 indexes_uv_to_points.push_back (u_null);
787 indexes_uv_to_points.push_back (u_null);
788 indexes_uv_to_points.push_back (u_null);
790 visibility[idx_face] =
false;
800 if (visibility.size () - cpt_invisible !=0)
807 std::vector<float> neighborsSquaredDistance;
811 for (
int idx_pcam = 0 ; idx_pcam <= current_cam ; ++idx_pcam)
814 for (
int idx_face = 0; idx_face < static_cast<int> (mesh.
tex_polygons[idx_pcam].size ()); ++idx_face)
817 if (idx_pcam == current_cam && !visibility[idx_face])
830 if (isFaceProjected (cameras[current_cam],
831 (*camera_cloud)[mesh.
tex_polygons[idx_pcam][idx_face].vertices[0]],
832 (*camera_cloud)[mesh.
tex_polygons[idx_pcam][idx_face].vertices[1]],
833 (*camera_cloud)[mesh.
tex_polygons[idx_pcam][idx_face].vertices[2]],
843 getTriangleCircumcscribedCircleCentroid(uv_coord1, uv_coord2, uv_coord3, center, radius);
846 if (kdtree.
radiusSearch (center, radius, idxNeighbors, neighborsSquaredDistance) > 0 )
849 for (
const auto &idxNeighbor : idxNeighbors)
851 if (std::max ((*camera_cloud)[mesh.
tex_polygons[idx_pcam][idx_face].vertices[0]].z,
852 std::max ((*camera_cloud)[mesh.
tex_polygons[idx_pcam][idx_face].vertices[1]].z,
853 (*camera_cloud)[mesh.
tex_polygons[idx_pcam][idx_face].vertices[2]].z))
854 < (*camera_cloud)[indexes_uv_to_points[idxNeighbor].idx_cloud].z)
857 if (checkPointInsideTriangle(uv_coord1, uv_coord2, uv_coord3, (*projections)[idxNeighbor]))
860 visibility[indexes_uv_to_points[idxNeighbor].idx_face] =
false;
877 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > dummy_container;
882 std::vector<pcl::Vertices> occluded_faces;
883 occluded_faces.resize (visibility.size ());
884 std::vector<pcl::Vertices> visible_faces;
885 visible_faces.resize (visibility.size ());
887 int cpt_occluded_faces = 0;
888 int cpt_visible_faces = 0;
890 for (std::size_t idx_face = 0 ; idx_face < visibility.size () ; ++idx_face)
892 if (visibility[idx_face])
895 mesh.
tex_coordinates[current_cam][cpt_visible_faces * 3](0) = (*projections)[idx_face*3].x;
896 mesh.
tex_coordinates[current_cam][cpt_visible_faces * 3](1) = (*projections)[idx_face*3].y;
898 mesh.
tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](0) = (*projections)[idx_face*3 + 1].x;
899 mesh.
tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](1) = (*projections)[idx_face*3 + 1].y;
901 mesh.
tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](0) = (*projections)[idx_face*3 + 2].x;
902 mesh.
tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](1) = (*projections)[idx_face*3 + 2].y;
904 visible_faces[cpt_visible_faces] = mesh.
tex_polygons[current_cam][idx_face];
911 occluded_faces[cpt_occluded_faces] = mesh.
tex_polygons[current_cam][idx_face];
912 cpt_occluded_faces++;
917 occluded_faces.resize (cpt_occluded_faces);
920 visible_faces.resize (cpt_visible_faces);
931 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > dummy_container;
936 for(std::size_t idx_face = 0 ; idx_face < mesh.
tex_polygons[cameras.size()].size() ; ++idx_face)
938 Eigen::Vector2f UV1, UV2, UV3;
939 UV1(0) = -1.0; UV1(1) = -1.0;
940 UV2(0) = -1.0; UV2(1) = -1.0;
941 UV3(0) = -1.0; UV3(1) = -1.0;
950 template<
typename Po
intInT>
inline void
955 ptB.
x = p2.
x - p1.
x; ptB.
y = p2.
y - p1.
y;
956 ptC.
x = p3.
x - p1.
x; ptC.
y = p3.
y - p1.
y;
958 double D = 2.0*(ptB.
x*ptC.
y - ptB.
y*ptC.
x);
963 circomcenter.
x = p1.
x;
964 circomcenter.
y = p1.
y;
969 double bx2 = ptB.
x * ptB.
x;
970 double by2 = ptB.
y * ptB.
y;
971 double cx2 = ptC.
x * ptC.
x;
972 double cy2 = ptC.
y * ptC.
y;
975 circomcenter.
x =
static_cast<float> (p1.
x + (ptC.
y*(bx2 + by2) - ptB.
y*(cx2 + cy2)) / D);
976 circomcenter.
y =
static_cast<float> (p1.
y + (ptB.
x*(cx2 + cy2) - ptC.
x*(bx2 + by2)) / D);
979 radius = std::sqrt( (circomcenter.
x - p1.
x)*(circomcenter.
x - p1.
x) + (circomcenter.
y - p1.
y)*(circomcenter.
y - p1.
y));
983 template<
typename Po
intInT>
inline void
987 circumcenter.
x =
static_cast<float> (p1.
x + p2.
x + p3.
x ) / 3;
988 circumcenter.
y =
static_cast<float> (p1.
y + p2.
y + p3.
y ) / 3;
989 double r1 = (circumcenter.
x - p1.
x) * (circumcenter.
x - p1.
x) + (circumcenter.
y - p1.
y) * (circumcenter.
y - p1.
y) ;
990 double r2 = (circumcenter.
x - p2.
x) * (circumcenter.
x - p2.
x) + (circumcenter.
y - p2.
y) * (circumcenter.
y - p2.
y) ;
991 double r3 = (circumcenter.
x - p3.
x) * (circumcenter.
x - p3.
x) + (circumcenter.
y - p3.
y) * (circumcenter.
y - p3.
y) ;
994 radius = std::sqrt( std::max( r1, std::max( r2, r3) )) ;
999 template<
typename Po
intInT>
inline bool
1005 double sizeX = cam.
width;
1006 double sizeY = cam.
height;
1017 double focal_x, focal_y;
1028 UV_coordinates.
x =
static_cast<float> ((focal_x * (pt.x / pt.z) + cx) / sizeX);
1029 UV_coordinates.
y = 1.0f -
static_cast<float> ((focal_y * (pt.y / pt.z) + cy) / sizeY);
1032 if (UV_coordinates.
x >= 0.0 && UV_coordinates.
x <= 1.0 && UV_coordinates.
y >= 0.0 && UV_coordinates.
y <= 1.0)
1037 UV_coordinates.
x = -1.0f;
1038 UV_coordinates.
y = -1.0f;
1043 template<
typename Po
intInT>
inline bool
1047 Eigen::Vector2d v0, v1, v2;
1048 v0(0) = p3.
x - p1.
x; v0(1) = p3.
y - p1.
y;
1049 v1(0) = p2.
x - p1.
x; v1(1) = p2.
y - p1.
y;
1050 v2(0) = pt.
x - p1.
x; v2(1) = pt.
y - p1.
y;
1053 double dot00 = v0.dot(v0);
1054 double dot01 = v0.dot(v1);
1055 double dot02 = v0.dot(v2);
1056 double dot11 = v1.dot(v1);
1057 double dot12 = v1.dot(v2);
1060 double invDenom = 1.0 / (dot00*dot11 - dot01*dot01);
1061 double u = (dot11*dot02 - dot01*dot12) * invDenom;
1062 double v = (dot00*dot12 - dot01*dot02) * invDenom;
1065 return ((u >= 0) && (v >= 0) && (u + v < 1));
1069 template<
typename Po
intInT>
inline bool
1072 return (getPointUVCoordinates(p1, camera, proj1)
1074 getPointUVCoordinates(p2, camera, proj2)
1076 getPointUVCoordinates(p3, camera, proj3)
1080 #define PCL_INSTANTIATE_TextureMapping(T) \
1081 template class PCL_EXPORTS pcl::TextureMapping<T>;