Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of Willow Garage, Inc. nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 * 00034 * $Id$ 00035 * 00036 */ 00037 00038 #ifndef PCL_SURFACE_IMPL_TEXTURE_MAPPING_HPP_ 00039 #define PCL_SURFACE_IMPL_TEXTURE_MAPPING_HPP_ 00040 00041 #include <pcl/common/distances.h> 00042 #include <pcl/surface/texture_mapping.h> 00043 00044 /////////////////////////////////////////////////////////////////////////////////////////////// 00045 template<typename PointInT> std::vector<Eigen::Vector2f> 00046 pcl::TextureMapping<PointInT>::mapTexture2Face ( 00047 const Eigen::Vector3f &p1, 00048 const Eigen::Vector3f &p2, 00049 const Eigen::Vector3f &p3) 00050 { 00051 std::vector<Eigen::Vector2f> tex_coordinates; 00052 // process for each face 00053 Eigen::Vector3f p1p2 (p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]); 00054 Eigen::Vector3f p1p3 (p3[0] - p1[0], p3[1] - p1[1], p3[2] - p1[2]); 00055 Eigen::Vector3f p2p3 (p3[0] - p2[0], p3[1] - p2[1], p3[2] - p2[2]); 00056 00057 // Normalize 00058 p1p2 = p1p2 / std::sqrt (p1p2.dot (p1p2)); 00059 p1p3 = p1p3 / std::sqrt (p1p3.dot (p1p3)); 00060 p2p3 = p2p3 / std::sqrt (p2p3.dot (p2p3)); 00061 00062 // compute vector normal of a face 00063 Eigen::Vector3f f_normal = p1p2.cross (p1p3); 00064 f_normal = f_normal / std::sqrt (f_normal.dot (f_normal)); 00065 00066 // project vector field onto the face: vector v1_projected = v1 - Dot(v1, n) * n; 00067 Eigen::Vector3f f_vector_field = vector_field_ - vector_field_.dot (f_normal) * f_normal; 00068 00069 // Normalize 00070 f_vector_field = f_vector_field / std::sqrt (f_vector_field.dot (f_vector_field)); 00071 00072 // texture coordinates 00073 Eigen::Vector2f tp1, tp2, tp3; 00074 00075 double alpha = std::acos (f_vector_field.dot (p1p2)); 00076 00077 // distance between 3 vertices of triangles 00078 double e1 = (p2 - p3).norm () / f_; 00079 double e2 = (p1 - p3).norm () / f_; 00080 double e3 = (p1 - p2).norm () / f_; 00081 00082 // initialize 00083 tp1[0] = 0.0; 00084 tp1[1] = 0.0; 00085 00086 tp2[0] = static_cast<float> (e3); 00087 tp2[1] = 0.0; 00088 00089 // determine texture coordinate tp3; 00090 double cos_p1 = (e2 * e2 + e3 * e3 - e1 * e1) / (2 * e2 * e3); 00091 double sin_p1 = sqrt (1 - (cos_p1 * cos_p1)); 00092 00093 tp3[0] = static_cast<float> (cos_p1 * e2); 00094 tp3[1] = static_cast<float> (sin_p1 * e2); 00095 00096 // rotating by alpha (angle between V and pp1 & pp2) 00097 Eigen::Vector2f r_tp2, r_tp3; 00098 r_tp2[0] = static_cast<float> (tp2[0] * std::cos (alpha) - tp2[1] * std::sin (alpha)); 00099 r_tp2[1] = static_cast<float> (tp2[0] * std::sin (alpha) + tp2[1] * std::cos (alpha)); 00100 00101 r_tp3[0] = static_cast<float> (tp3[0] * std::cos (alpha) - tp3[1] * std::sin (alpha)); 00102 r_tp3[1] = static_cast<float> (tp3[0] * std::sin (alpha) + tp3[1] * std::cos (alpha)); 00103 00104 // shifting 00105 tp1[0] = tp1[0]; 00106 tp2[0] = r_tp2[0]; 00107 tp3[0] = r_tp3[0]; 00108 tp1[1] = tp1[1]; 00109 tp2[1] = r_tp2[1]; 00110 tp3[1] = r_tp3[1]; 00111 00112 float min_x = tp1[0]; 00113 float min_y = tp1[1]; 00114 if (min_x > tp2[0]) 00115 min_x = tp2[0]; 00116 if (min_x > tp3[0]) 00117 min_x = tp3[0]; 00118 if (min_y > tp2[1]) 00119 min_y = tp2[1]; 00120 if (min_y > tp3[1]) 00121 min_y = tp3[1]; 00122 00123 if (min_x < 0) 00124 { 00125 tp1[0] = tp1[0] - min_x; 00126 tp2[0] = tp2[0] - min_x; 00127 tp3[0] = tp3[0] - min_x; 00128 } 00129 if (min_y < 0) 00130 { 00131 tp1[1] = tp1[1] - min_y; 00132 tp2[1] = tp2[1] - min_y; 00133 tp3[1] = tp3[1] - min_y; 00134 } 00135 00136 tex_coordinates.push_back (tp1); 00137 tex_coordinates.push_back (tp2); 00138 tex_coordinates.push_back (tp3); 00139 return (tex_coordinates); 00140 } 00141 00142 /////////////////////////////////////////////////////////////////////////////////////////////// 00143 template<typename PointInT> void 00144 pcl::TextureMapping<PointInT>::mapTexture2Mesh (pcl::TextureMesh &tex_mesh) 00145 { 00146 // mesh information 00147 int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height; 00148 int point_size = static_cast<int> (tex_mesh.cloud.data.size ()) / nr_points; 00149 00150 // temporary PointXYZ 00151 float x, y, z; 00152 // temporary face 00153 Eigen::Vector3f facet[3]; 00154 00155 // texture coordinates for each mesh 00156 std::vector<std::vector<Eigen::Vector2f> > texture_map; 00157 00158 for (size_t m = 0; m < tex_mesh.tex_polygons.size (); ++m) 00159 { 00160 // texture coordinates for each mesh 00161 std::vector<Eigen::Vector2f> texture_map_tmp; 00162 00163 // processing for each face 00164 for (size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i) 00165 { 00166 size_t idx; 00167 00168 // get facet information 00169 for (size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j) 00170 { 00171 idx = tex_mesh.tex_polygons[m][i].vertices[j]; 00172 memcpy (&x, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float)); 00173 memcpy (&y, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float)); 00174 memcpy (&z, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float)); 00175 facet[j][0] = x; 00176 facet[j][1] = y; 00177 facet[j][2] = z; 00178 } 00179 00180 // get texture coordinates of each face 00181 std::vector<Eigen::Vector2f> tex_coordinates = mapTexture2Face (facet[0], facet[1], facet[2]); 00182 for (size_t n = 0; n < tex_coordinates.size (); ++n) 00183 texture_map_tmp.push_back (tex_coordinates[n]); 00184 }// end faces 00185 00186 // texture materials 00187 std::stringstream tex_name; 00188 tex_name << "material_" << m; 00189 tex_name >> tex_material_.tex_name; 00190 tex_material_.tex_file = tex_files_[m]; 00191 tex_mesh.tex_materials.push_back (tex_material_); 00192 00193 // texture coordinates 00194 tex_mesh.tex_coordinates.push_back (texture_map_tmp); 00195 }// end meshes 00196 } 00197 00198 /////////////////////////////////////////////////////////////////////////////////////////////// 00199 template<typename PointInT> void 00200 pcl::TextureMapping<PointInT>::mapTexture2MeshUV (pcl::TextureMesh &tex_mesh) 00201 { 00202 // mesh information 00203 int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height; 00204 int point_size = static_cast<int> (tex_mesh.cloud.data.size ()) / nr_points; 00205 00206 float x_lowest = 100000; 00207 float x_highest = 0; 00208 float y_lowest = 100000; 00209 //float y_highest = 0 ; 00210 float z_lowest = 100000; 00211 float z_highest = 0; 00212 float x_, y_, z_; 00213 00214 for (int i = 0; i < nr_points; ++i) 00215 { 00216 memcpy (&x_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float)); 00217 memcpy (&y_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float)); 00218 memcpy (&z_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float)); 00219 // x 00220 if (x_ <= x_lowest) 00221 x_lowest = x_; 00222 if (x_ > x_lowest) 00223 x_highest = x_; 00224 00225 // y 00226 if (y_ <= y_lowest) 00227 y_lowest = y_; 00228 //if (y_ > y_lowest) y_highest = y_; 00229 00230 // z 00231 if (z_ <= z_lowest) 00232 z_lowest = z_; 00233 if (z_ > z_lowest) 00234 z_highest = z_; 00235 } 00236 // x 00237 float x_range = (x_lowest - x_highest) * -1; 00238 float x_offset = 0 - x_lowest; 00239 // x 00240 // float y_range = (y_lowest - y_highest)*-1; 00241 // float y_offset = 0 - y_lowest; 00242 // z 00243 float z_range = (z_lowest - z_highest) * -1; 00244 float z_offset = 0 - z_lowest; 00245 00246 // texture coordinates for each mesh 00247 std::vector<std::vector<Eigen::Vector2f> > texture_map; 00248 00249 for (size_t m = 0; m < tex_mesh.tex_polygons.size (); ++m) 00250 { 00251 // texture coordinates for each mesh 00252 std::vector<Eigen::Vector2f> texture_map_tmp; 00253 00254 // processing for each face 00255 for (size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i) 00256 { 00257 size_t idx; 00258 Eigen::Vector2f tmp_VT; 00259 for (size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j) 00260 { 00261 idx = tex_mesh.tex_polygons[m][i].vertices[j]; 00262 memcpy (&x_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float)); 00263 memcpy (&y_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float)); 00264 memcpy (&z_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float)); 00265 00266 // calculate uv coordinates 00267 tmp_VT[0] = (x_ + x_offset) / x_range; 00268 tmp_VT[1] = (z_ + z_offset) / z_range; 00269 texture_map_tmp.push_back (tmp_VT); 00270 } 00271 }// end faces 00272 00273 // texture materials 00274 std::stringstream tex_name; 00275 tex_name << "material_" << m; 00276 tex_name >> tex_material_.tex_name; 00277 tex_material_.tex_file = tex_files_[m]; 00278 tex_mesh.tex_materials.push_back (tex_material_); 00279 00280 // texture coordinates 00281 tex_mesh.tex_coordinates.push_back (texture_map_tmp); 00282 }// end meshes 00283 } 00284 00285 /////////////////////////////////////////////////////////////////////////////////////////////// 00286 template<typename PointInT> void 00287 pcl::TextureMapping<PointInT>::mapMultipleTexturesToMeshUV (pcl::TextureMesh &tex_mesh, pcl::texture_mapping::CameraVector &cams) 00288 { 00289 00290 if (tex_mesh.tex_polygons.size () != cams.size () + 1) 00291 { 00292 PCL_ERROR ("The mesh should be divided into nbCamera+1 sub-meshes.\n"); 00293 PCL_ERROR ("You provided %d cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.tex_polygons.size ()); 00294 return; 00295 } 00296 00297 PCL_INFO ("You provided %d cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.tex_polygons.size ()); 00298 00299 pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud (new pcl::PointCloud<pcl::PointXYZ>); 00300 pcl::PointCloud<pcl::PointXYZ>::Ptr camera_transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>); 00301 00302 // convert mesh's cloud to pcl format for ease 00303 pcl::fromPCLPointCloud2 (tex_mesh.cloud, *originalCloud); 00304 00305 // texture coordinates for each mesh 00306 std::vector<std::vector<Eigen::Vector2f> > texture_map; 00307 00308 for (size_t m = 0; m < cams.size (); ++m) 00309 { 00310 // get current camera parameters 00311 Camera current_cam = cams[m]; 00312 00313 // get camera transform 00314 Eigen::Affine3f cam_trans = current_cam.pose; 00315 00316 // transform cloud into current camera frame 00317 pcl::transformPointCloud (*originalCloud, *camera_transformed_cloud, cam_trans.inverse ()); 00318 00319 // vector of texture coordinates for each face 00320 std::vector<Eigen::Vector2f> texture_map_tmp; 00321 00322 // processing each face visible by this camera 00323 pcl::PointXYZ pt; 00324 size_t idx; 00325 for (size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i) 00326 { 00327 Eigen::Vector2f tmp_VT; 00328 // for each point of this face 00329 for (size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j) 00330 { 00331 // get point 00332 idx = tex_mesh.tex_polygons[m][i].vertices[j]; 00333 pt = camera_transformed_cloud->points[idx]; 00334 00335 // compute UV coordinates for this point 00336 getPointUVCoordinates (pt, current_cam, tmp_VT); 00337 texture_map_tmp.push_back (tmp_VT); 00338 00339 }// end points 00340 }// end faces 00341 00342 // texture materials 00343 std::stringstream tex_name; 00344 tex_name << "material_" << m; 00345 tex_name >> tex_material_.tex_name; 00346 tex_material_.tex_file = current_cam.texture_file; 00347 tex_mesh.tex_materials.push_back (tex_material_); 00348 00349 // texture coordinates 00350 tex_mesh.tex_coordinates.push_back (texture_map_tmp); 00351 }// end cameras 00352 00353 // push on extra empty UV map (for unseen faces) so that obj writer does not crash! 00354 std::vector<Eigen::Vector2f> texture_map_tmp; 00355 for (size_t i = 0; i < tex_mesh.tex_polygons[cams.size ()].size (); ++i) 00356 for (size_t j = 0; j < tex_mesh.tex_polygons[cams.size ()][i].vertices.size (); ++j) 00357 { 00358 Eigen::Vector2f tmp_VT; 00359 tmp_VT[0] = -1; 00360 tmp_VT[1] = -1; 00361 texture_map_tmp.push_back (tmp_VT); 00362 } 00363 00364 tex_mesh.tex_coordinates.push_back (texture_map_tmp); 00365 00366 // push on an extra dummy material for the same reason 00367 std::stringstream tex_name; 00368 tex_name << "material_" << cams.size (); 00369 tex_name >> tex_material_.tex_name; 00370 tex_material_.tex_file = "occluded.jpg"; 00371 tex_mesh.tex_materials.push_back (tex_material_); 00372 00373 } 00374 00375 /////////////////////////////////////////////////////////////////////////////////////////////// 00376 template<typename PointInT> bool 00377 pcl::TextureMapping<PointInT>::isPointOccluded (const pcl::PointXYZ &pt, OctreePtr octree) 00378 { 00379 Eigen::Vector3f direction; 00380 direction (0) = pt.x; 00381 direction (1) = pt.y; 00382 direction (2) = pt.z; 00383 00384 std::vector<int> indices; 00385 00386 PointCloudConstPtr cloud (new PointCloud()); 00387 cloud = octree->getInputCloud(); 00388 00389 double distance_threshold = octree->getResolution(); 00390 00391 // raytrace 00392 octree->getIntersectedVoxelIndices(direction, -direction, indices); 00393 00394 int nbocc = static_cast<int> (indices.size ()); 00395 for (size_t j = 0; j < indices.size (); j++) 00396 { 00397 // if intersected point is on the over side of the camera 00398 if (pt.z * cloud->points[indices[j]].z < 0) 00399 { 00400 nbocc--; 00401 continue; 00402 } 00403 00404 if (fabs (cloud->points[indices[j]].z - pt.z) <= distance_threshold) 00405 { 00406 // points are very close to each-other, we do not consider the occlusion 00407 nbocc--; 00408 } 00409 } 00410 00411 if (nbocc == 0) 00412 return (false); 00413 else 00414 return (true); 00415 } 00416 00417 /////////////////////////////////////////////////////////////////////////////////////////////// 00418 template<typename PointInT> void 00419 pcl::TextureMapping<PointInT>::removeOccludedPoints (const PointCloudPtr &input_cloud, 00420 PointCloudPtr &filtered_cloud, 00421 const double octree_voxel_size, std::vector<int> &visible_indices, 00422 std::vector<int> &occluded_indices) 00423 { 00424 // variable used to filter occluded points by depth 00425 double maxDeltaZ = octree_voxel_size; 00426 00427 // create an octree to perform rayTracing 00428 OctreePtr octree (new Octree (octree_voxel_size)); 00429 // create octree structure 00430 octree->setInputCloud (input_cloud); 00431 // update bounding box automatically 00432 octree->defineBoundingBox (); 00433 // add points in the tree 00434 octree->addPointsFromInputCloud (); 00435 00436 visible_indices.clear (); 00437 00438 // for each point of the cloud, raycast toward camera and check intersected voxels. 00439 Eigen::Vector3f direction; 00440 std::vector<int> indices; 00441 for (size_t i = 0; i < input_cloud->points.size (); ++i) 00442 { 00443 direction (0) = input_cloud->points[i].x; 00444 direction (1) = input_cloud->points[i].y; 00445 direction (2) = input_cloud->points[i].z; 00446 00447 // if point is not occluded 00448 octree->getIntersectedVoxelIndices (direction, -direction, indices); 00449 00450 int nbocc = static_cast<int> (indices.size ()); 00451 for (size_t j = 0; j < indices.size (); j++) 00452 { 00453 // if intersected point is on the over side of the camera 00454 if (input_cloud->points[i].z * input_cloud->points[indices[j]].z < 0) 00455 { 00456 nbocc--; 00457 continue; 00458 } 00459 00460 if (fabs (input_cloud->points[indices[j]].z - input_cloud->points[i].z) <= maxDeltaZ) 00461 { 00462 // points are very close to each-other, we do not consider the occlusion 00463 nbocc--; 00464 } 00465 } 00466 00467 if (nbocc == 0) 00468 { 00469 // point is added in the filtered mesh 00470 filtered_cloud->points.push_back (input_cloud->points[i]); 00471 visible_indices.push_back (static_cast<int> (i)); 00472 } 00473 else 00474 { 00475 occluded_indices.push_back (static_cast<int> (i)); 00476 } 00477 } 00478 00479 } 00480 00481 /////////////////////////////////////////////////////////////////////////////////////////////// 00482 template<typename PointInT> void 00483 pcl::TextureMapping<PointInT>::removeOccludedPoints (const pcl::TextureMesh &tex_mesh, pcl::TextureMesh &cleaned_mesh, const double octree_voxel_size) 00484 { 00485 // copy mesh 00486 cleaned_mesh = tex_mesh; 00487 00488 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); 00489 pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>); 00490 00491 // load points into a PCL format 00492 pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud); 00493 00494 std::vector<int> visible, occluded; 00495 removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded); 00496 00497 // Now that we know which points are visible, let's iterate over each face. 00498 // if the face has one invisible point => out! 00499 for (size_t polygons = 0; polygons < cleaned_mesh.tex_polygons.size (); ++polygons) 00500 { 00501 // remove all faces from cleaned mesh 00502 cleaned_mesh.tex_polygons[polygons].clear (); 00503 // iterate over faces 00504 for (size_t faces = 0; faces < tex_mesh.tex_polygons[polygons].size (); ++faces) 00505 { 00506 // check if all the face's points are visible 00507 bool faceIsVisible = true; 00508 std::vector<int>::iterator it; 00509 00510 // iterate over face's vertex 00511 for (size_t points = 0; points < tex_mesh.tex_polygons[polygons][faces].vertices.size (); ++points) 00512 { 00513 it = find (occluded.begin (), occluded.end (), tex_mesh.tex_polygons[polygons][faces].vertices[points]); 00514 00515 if (it == occluded.end ()) 00516 { 00517 // point is not in the occluded vector 00518 // PCL_INFO (" VISIBLE!\n"); 00519 } 00520 else 00521 { 00522 // point was occluded 00523 // PCL_INFO(" OCCLUDED!\n"); 00524 faceIsVisible = false; 00525 } 00526 } 00527 00528 if (faceIsVisible) 00529 { 00530 cleaned_mesh.tex_polygons[polygons].push_back (tex_mesh.tex_polygons[polygons][faces]); 00531 } 00532 00533 } 00534 } 00535 } 00536 00537 /////////////////////////////////////////////////////////////////////////////////////////////// 00538 template<typename PointInT> void 00539 pcl::TextureMapping<PointInT>::removeOccludedPoints (const pcl::TextureMesh &tex_mesh, PointCloudPtr &filtered_cloud, 00540 const double octree_voxel_size) 00541 { 00542 PointCloudPtr cloud (new PointCloud); 00543 00544 // load points into a PCL format 00545 pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud); 00546 00547 std::vector<int> visible, occluded; 00548 removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded); 00549 00550 } 00551 00552 /////////////////////////////////////////////////////////////////////////////////////////////// 00553 template<typename PointInT> int 00554 pcl::TextureMapping<PointInT>::sortFacesByCamera (pcl::TextureMesh &tex_mesh, pcl::TextureMesh &sorted_mesh, 00555 const pcl::texture_mapping::CameraVector &cameras, const double octree_voxel_size, 00556 PointCloud &visible_pts) 00557 { 00558 if (tex_mesh.tex_polygons.size () != 1) 00559 { 00560 PCL_ERROR ("The mesh must contain only 1 sub-mesh!\n"); 00561 return (-1); 00562 } 00563 00564 if (cameras.size () == 0) 00565 { 00566 PCL_ERROR ("Must provide at least one camera info!\n"); 00567 return (-1); 00568 } 00569 00570 // copy mesh 00571 sorted_mesh = tex_mesh; 00572 // clear polygons from cleaned_mesh 00573 sorted_mesh.tex_polygons.clear (); 00574 00575 pcl::PointCloud<pcl::PointXYZ>::Ptr original_cloud (new pcl::PointCloud<pcl::PointXYZ>); 00576 pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>); 00577 pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>); 00578 00579 // load points into a PCL format 00580 pcl::fromPCLPointCloud2 (tex_mesh.cloud, *original_cloud); 00581 00582 // for each camera 00583 for (size_t cam = 0; cam < cameras.size (); ++cam) 00584 { 00585 // get camera pose as transform 00586 Eigen::Affine3f cam_trans = cameras[cam].pose; 00587 00588 // transform original cloud in camera coordinates 00589 pcl::transformPointCloud (*original_cloud, *transformed_cloud, cam_trans.inverse ()); 00590 00591 // find occlusions on transformed cloud 00592 std::vector<int> visible, occluded; 00593 removeOccludedPoints (transformed_cloud, filtered_cloud, octree_voxel_size, visible, occluded); 00594 visible_pts = *filtered_cloud; 00595 00596 // find visible faces => add them to polygon N for camera N 00597 // add polygon group for current camera in clean 00598 std::vector<pcl::Vertices> visibleFaces_currentCam; 00599 // iterate over the faces of the current mesh 00600 for (size_t faces = 0; faces < tex_mesh.tex_polygons[0].size (); ++faces) 00601 { 00602 // check if all the face's points are visible 00603 bool faceIsVisible = true; 00604 std::vector<int>::iterator it; 00605 00606 // iterate over face's vertex 00607 for (size_t current_pt_indice = 0; faceIsVisible && current_pt_indice < tex_mesh.tex_polygons[0][faces].vertices.size (); ++current_pt_indice) 00608 { 00609 // TODO this is far too long! Better create an helper function that raycasts here. 00610 it = find (occluded.begin (), occluded.end (), tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]); 00611 00612 if (it == occluded.end ()) 00613 { 00614 // point is not occluded 00615 // does it land on the camera's image plane? 00616 pcl::PointXYZ pt = transformed_cloud->points[tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]]; 00617 Eigen::Vector2f dummy_UV; 00618 if (!getPointUVCoordinates (pt, cameras[cam], dummy_UV)) 00619 { 00620 // point is not visible by the camera 00621 faceIsVisible = false; 00622 } 00623 } 00624 else 00625 { 00626 faceIsVisible = false; 00627 } 00628 } 00629 00630 if (faceIsVisible) 00631 { 00632 // push current visible face into the sorted mesh 00633 visibleFaces_currentCam.push_back (tex_mesh.tex_polygons[0][faces]); 00634 // remove it from the unsorted mesh 00635 tex_mesh.tex_polygons[0].erase (tex_mesh.tex_polygons[0].begin () + faces); 00636 faces--; 00637 } 00638 00639 } 00640 sorted_mesh.tex_polygons.push_back (visibleFaces_currentCam); 00641 } 00642 00643 // we should only have occluded and non-visible faces left in tex_mesh.tex_polygons[0] 00644 // we need to add them as an extra polygon in the sorted mesh 00645 sorted_mesh.tex_polygons.push_back (tex_mesh.tex_polygons[0]); 00646 return (0); 00647 } 00648 00649 /////////////////////////////////////////////////////////////////////////////////////////////// 00650 template<typename PointInT> void 00651 pcl::TextureMapping<PointInT>::showOcclusions (const PointCloudPtr &input_cloud, 00652 pcl::PointCloud<pcl::PointXYZI>::Ptr &colored_cloud, 00653 const double octree_voxel_size, const bool show_nb_occlusions, 00654 const int max_occlusions) 00655 { 00656 // variable used to filter occluded points by depth 00657 double maxDeltaZ = octree_voxel_size * 2.0; 00658 00659 // create an octree to perform rayTracing 00660 pcl::octree::OctreePointCloudSearch<PointInT> *octree; 00661 octree = new pcl::octree::OctreePointCloudSearch<PointInT> (octree_voxel_size); 00662 // create octree structure 00663 octree->setInputCloud (input_cloud); 00664 // update bounding box automatically 00665 octree->defineBoundingBox (); 00666 // add points in the tree 00667 octree->addPointsFromInputCloud (); 00668 00669 // ray direction 00670 Eigen::Vector3f direction; 00671 00672 std::vector<int> indices; 00673 // point from where we ray-trace 00674 pcl::PointXYZI pt; 00675 00676 std::vector<double> zDist; 00677 std::vector<double> ptDist; 00678 // for each point of the cloud, ray-trace toward the camera and check intersected voxels. 00679 for (size_t i = 0; i < input_cloud->points.size (); ++i) 00680 { 00681 direction (0) = input_cloud->points[i].x; 00682 pt.x = input_cloud->points[i].x; 00683 direction (1) = input_cloud->points[i].y; 00684 pt.y = input_cloud->points[i].y; 00685 direction (2) = input_cloud->points[i].z; 00686 pt.z = input_cloud->points[i].z; 00687 00688 // get number of occlusions for that point 00689 indices.clear (); 00690 int nbocc = octree->getIntersectedVoxelIndices (direction, -direction, indices); 00691 00692 nbocc = static_cast<int> (indices.size ()); 00693 00694 // TODO need to clean this up and find tricks to get remove aliasaing effect on planes 00695 for (size_t j = 0; j < indices.size (); j++) 00696 { 00697 // if intersected point is on the over side of the camera 00698 if (pt.z * input_cloud->points[indices[j]].z < 0) 00699 { 00700 nbocc--; 00701 } 00702 else if (fabs (input_cloud->points[indices[j]].z - pt.z) <= maxDeltaZ) 00703 { 00704 // points are very close to each-other, we do not consider the occlusion 00705 nbocc--; 00706 } 00707 else 00708 { 00709 zDist.push_back (fabs (input_cloud->points[indices[j]].z - pt.z)); 00710 ptDist.push_back (pcl::euclideanDistance (input_cloud->points[indices[j]], pt)); 00711 } 00712 } 00713 00714 if (show_nb_occlusions) 00715 (nbocc <= max_occlusions) ? (pt.intensity = static_cast<float> (nbocc)) : (pt.intensity = static_cast<float> (max_occlusions)); 00716 else 00717 (nbocc == 0) ? (pt.intensity = 0) : (pt.intensity = 1); 00718 00719 colored_cloud->points.push_back (pt); 00720 } 00721 00722 if (zDist.size () >= 2) 00723 { 00724 std::sort (zDist.begin (), zDist.end ()); 00725 std::sort (ptDist.begin (), ptDist.end ()); 00726 } 00727 } 00728 00729 /////////////////////////////////////////////////////////////////////////////////////////////// 00730 template<typename PointInT> void 00731 pcl::TextureMapping<PointInT>::showOcclusions (pcl::TextureMesh &tex_mesh, pcl::PointCloud<pcl::PointXYZI>::Ptr &colored_cloud, 00732 double octree_voxel_size, bool show_nb_occlusions, int max_occlusions) 00733 { 00734 // load points into a PCL format 00735 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>); 00736 pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud); 00737 00738 showOcclusions (cloud, colored_cloud, octree_voxel_size, show_nb_occlusions, max_occlusions); 00739 } 00740 00741 /////////////////////////////////////////////////////////////////////////////////////////////// 00742 template<typename PointInT> void 00743 pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras (pcl::TextureMesh &mesh, const pcl::texture_mapping::CameraVector &cameras) 00744 { 00745 00746 if (mesh.tex_polygons.size () != 1) 00747 return; 00748 00749 pcl::PointCloud<pcl::PointXYZ>::Ptr mesh_cloud (new pcl::PointCloud<pcl::PointXYZ>); 00750 00751 pcl::fromPCLPointCloud2 (mesh.cloud, *mesh_cloud); 00752 00753 std::vector<pcl::Vertices> faces; 00754 00755 for (int current_cam = 0; current_cam < static_cast<int> (cameras.size ()); ++current_cam) 00756 { 00757 PCL_INFO ("Processing camera %d of %d.\n", current_cam+1, cameras.size ()); 00758 00759 // transform mesh into camera's frame 00760 pcl::PointCloud<pcl::PointXYZ>::Ptr camera_cloud (new pcl::PointCloud<pcl::PointXYZ>); 00761 pcl::transformPointCloud (*mesh_cloud, *camera_cloud, cameras[current_cam].pose.inverse ()); 00762 00763 // CREATE UV MAP FOR CURRENT FACES 00764 pcl::PointCloud<pcl::PointXY>::Ptr projections (new pcl::PointCloud<pcl::PointXY>); 00765 std::vector<pcl::Vertices>::iterator current_face; 00766 std::vector<bool> visibility; 00767 visibility.resize (mesh.tex_polygons[current_cam].size ()); 00768 std::vector<UvIndex> indexes_uv_to_points; 00769 // for each current face 00770 00771 //TODO change this 00772 pcl::PointXY nan_point; 00773 nan_point.x = std::numeric_limits<float>::quiet_NaN (); 00774 nan_point.y = std::numeric_limits<float>::quiet_NaN (); 00775 UvIndex u_null; 00776 u_null.idx_cloud = -1; 00777 u_null.idx_face = -1; 00778 00779 int cpt_invisible=0; 00780 for (int idx_face = 0; idx_face < static_cast<int> (mesh.tex_polygons[current_cam].size ()); ++idx_face) 00781 { 00782 //project each vertice, if one is out of view, stop 00783 pcl::PointXY uv_coord1; 00784 pcl::PointXY uv_coord2; 00785 pcl::PointXY uv_coord3; 00786 00787 if (isFaceProjected (cameras[current_cam], 00788 camera_cloud->points[mesh.tex_polygons[current_cam][idx_face].vertices[0]], 00789 camera_cloud->points[mesh.tex_polygons[current_cam][idx_face].vertices[1]], 00790 camera_cloud->points[mesh.tex_polygons[current_cam][idx_face].vertices[2]], 00791 uv_coord1, 00792 uv_coord2, 00793 uv_coord3)) 00794 { 00795 // face is in the camera's FOV 00796 00797 // add UV coordinates 00798 projections->points.push_back (uv_coord1); 00799 projections->points.push_back (uv_coord2); 00800 projections->points.push_back (uv_coord3); 00801 00802 // remember corresponding face 00803 UvIndex u1, u2, u3; 00804 u1.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[0]; 00805 u2.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[1]; 00806 u3.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[2]; 00807 u1.idx_face = idx_face; u2.idx_face = idx_face; u3.idx_face = idx_face; 00808 indexes_uv_to_points.push_back (u1); 00809 indexes_uv_to_points.push_back (u2); 00810 indexes_uv_to_points.push_back (u3); 00811 00812 //keep track of visibility 00813 visibility[idx_face] = true; 00814 } 00815 else 00816 { 00817 projections->points.push_back (nan_point); 00818 projections->points.push_back (nan_point); 00819 projections->points.push_back (nan_point); 00820 indexes_uv_to_points.push_back (u_null); 00821 indexes_uv_to_points.push_back (u_null); 00822 indexes_uv_to_points.push_back (u_null); 00823 //keep track of visibility 00824 visibility[idx_face] = false; 00825 cpt_invisible++; 00826 } 00827 } 00828 00829 // projections contains all UV points of the current faces 00830 // indexes_uv_to_points links a uv point to its point in the camera cloud 00831 // visibility contains tells if a face was in the camera FOV (false = skip) 00832 00833 // TODO handle case were no face could be projected 00834 if (visibility.size () - cpt_invisible !=0) 00835 { 00836 //create kdtree 00837 pcl::KdTreeFLANN<pcl::PointXY> kdtree; 00838 kdtree.setInputCloud (projections); 00839 00840 std::vector<int> idxNeighbors; 00841 std::vector<float> neighborsSquaredDistance; 00842 // af first (idx_pcan < current_cam), check if some of the faces attached to previous cameras occlude the current faces 00843 // then (idx_pcam == current_cam), check for self occlusions. At this stage, we skip faces that were already marked as occluded 00844 cpt_invisible = 0; 00845 for (int idx_pcam = 0 ; idx_pcam <= current_cam ; ++idx_pcam) 00846 { 00847 // project all faces 00848 for (int idx_face = 0; idx_face < static_cast<int> (mesh.tex_polygons[idx_pcam].size ()); ++idx_face) 00849 { 00850 00851 if (idx_pcam == current_cam && !visibility[idx_face]) 00852 { 00853 // we are now checking for self occlusions within the current faces 00854 // the current face was already declared as occluded. 00855 // therefore, it cannot occlude another face anymore => we skip it 00856 continue; 00857 } 00858 00859 // project each vertice, if one is out of view, stop 00860 pcl::PointXY uv_coord1; 00861 pcl::PointXY uv_coord2; 00862 pcl::PointXY uv_coord3; 00863 00864 if (isFaceProjected (cameras[current_cam], 00865 camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[0]], 00866 camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[1]], 00867 camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[2]], 00868 uv_coord1, 00869 uv_coord2, 00870 uv_coord3)) 00871 { 00872 // face is in the camera's FOV 00873 //get its circumsribed circle 00874 double radius; 00875 pcl::PointXY center; 00876 // getTriangleCircumcenterAndSize (uv_coord1, uv_coord2, uv_coord3, center, radius); 00877 getTriangleCircumcscribedCircleCentroid(uv_coord1, uv_coord2, uv_coord3, center, radius); // this function yields faster results than getTriangleCircumcenterAndSize 00878 00879 // get points inside circ.circle 00880 if (kdtree.radiusSearch (center, radius, idxNeighbors, neighborsSquaredDistance) > 0 ) 00881 { 00882 // for each neighbor 00883 for (size_t i = 0; i < idxNeighbors.size (); ++i) 00884 { 00885 if (std::max (camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[0]].z, 00886 std::max (camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[1]].z, 00887 camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[2]].z)) 00888 < camera_cloud->points[indexes_uv_to_points[idxNeighbors[i]].idx_cloud].z) 00889 { 00890 // neighbor is farther than all the face's points. Check if it falls into the triangle 00891 if (checkPointInsideTriangle(uv_coord1, uv_coord2, uv_coord3, projections->points[idxNeighbors[i]])) 00892 { 00893 // current neighbor is inside triangle and is closer => the corresponding face 00894 visibility[indexes_uv_to_points[idxNeighbors[i]].idx_face] = false; 00895 cpt_invisible++; 00896 //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 00897 } 00898 } 00899 } 00900 } 00901 } 00902 } 00903 } 00904 } 00905 00906 // now, visibility is true for each face that belongs to the current camera 00907 // if a face is not visible, we push it into the next one. 00908 00909 if (static_cast<int> (mesh.tex_coordinates.size ()) <= current_cam) 00910 { 00911 std::vector<Eigen::Vector2f> dummy_container; 00912 mesh.tex_coordinates.push_back (dummy_container); 00913 } 00914 mesh.tex_coordinates[current_cam].resize (3 * visibility.size ()); 00915 00916 std::vector<pcl::Vertices> occluded_faces; 00917 occluded_faces.resize (visibility.size ()); 00918 std::vector<pcl::Vertices> visible_faces; 00919 visible_faces.resize (visibility.size ()); 00920 00921 int cpt_occluded_faces = 0; 00922 int cpt_visible_faces = 0; 00923 00924 for (size_t idx_face = 0 ; idx_face < visibility.size () ; ++idx_face) 00925 { 00926 if (visibility[idx_face]) 00927 { 00928 // face is visible by the current camera copy UV coordinates 00929 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3](0) = projections->points[idx_face*3].x; 00930 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3](1) = projections->points[idx_face*3].y; 00931 00932 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](0) = projections->points[idx_face*3 + 1].x; 00933 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](1) = projections->points[idx_face*3 + 1].y; 00934 00935 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](0) = projections->points[idx_face*3 + 2].x; 00936 mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](1) = projections->points[idx_face*3 + 2].y; 00937 00938 visible_faces[cpt_visible_faces] = mesh.tex_polygons[current_cam][idx_face]; 00939 00940 cpt_visible_faces++; 00941 } 00942 else 00943 { 00944 // face is occluded copy face into temp vector 00945 occluded_faces[cpt_occluded_faces] = mesh.tex_polygons[current_cam][idx_face]; 00946 cpt_occluded_faces++; 00947 } 00948 } 00949 mesh.tex_coordinates[current_cam].resize (cpt_visible_faces*3); 00950 00951 occluded_faces.resize (cpt_occluded_faces); 00952 mesh.tex_polygons.push_back (occluded_faces); 00953 00954 visible_faces.resize (cpt_visible_faces); 00955 mesh.tex_polygons[current_cam].clear (); 00956 mesh.tex_polygons[current_cam] = visible_faces; 00957 00958 int nb_faces = 0; 00959 for (int i = 0; i < static_cast<int> (mesh.tex_polygons.size ()); i++) 00960 nb_faces += static_cast<int> (mesh.tex_polygons[i].size ()); 00961 } 00962 00963 // we have been through all the cameras. 00964 // if any faces are left, they were not visible by any camera 00965 // we still need to produce uv coordinates for them 00966 00967 if (mesh.tex_coordinates.size() <= cameras.size ()) 00968 { 00969 std::vector<Eigen::Vector2f> dummy_container; 00970 mesh.tex_coordinates.push_back(dummy_container); 00971 } 00972 00973 00974 for(size_t idx_face = 0 ; idx_face < mesh.tex_polygons[cameras.size()].size() ; ++idx_face) 00975 { 00976 Eigen::Vector2f UV1, UV2, UV3; 00977 UV1(0) = -1.0; UV1(1) = -1.0; 00978 UV2(0) = -1.0; UV2(1) = -1.0; 00979 UV3(0) = -1.0; UV3(1) = -1.0; 00980 mesh.tex_coordinates[cameras.size()].push_back(UV1); 00981 mesh.tex_coordinates[cameras.size()].push_back(UV2); 00982 mesh.tex_coordinates[cameras.size()].push_back(UV3); 00983 } 00984 00985 } 00986 00987 /////////////////////////////////////////////////////////////////////////////////////////////// 00988 template<typename PointInT> inline void 00989 pcl::TextureMapping<PointInT>::getTriangleCircumcenterAndSize(const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circomcenter, double &radius) 00990 { 00991 // we simplify the problem by translating the triangle's origin to its first point 00992 pcl::PointXY ptB, ptC; 00993 ptB.x = p2.x - p1.x; ptB.y = p2.y - p1.y; // B'=B-A 00994 ptC.x = p3.x - p1.x; ptC.y = p3.y - p1.y; // C'=C-A 00995 00996 double D = 2.0*(ptB.x*ptC.y - ptB.y*ptC.x); // D'=2(B'x*C'y - B'y*C'x) 00997 00998 // Safety check to avoid division by zero 00999 if(D == 0) 01000 { 01001 circomcenter.x = p1.x; 01002 circomcenter.y = p1.y; 01003 } 01004 else 01005 { 01006 // compute squares once 01007 double bx2 = ptB.x * ptB.x; // B'x^2 01008 double by2 = ptB.y * ptB.y; // B'y^2 01009 double cx2 = ptC.x * ptC.x; // C'x^2 01010 double cy2 = ptC.y * ptC.y; // C'y^2 01011 01012 // compute circomcenter's coordinates (translate back to original coordinates) 01013 circomcenter.x = static_cast<float> (p1.x + (ptC.y*(bx2 + by2) - ptB.y*(cx2 + cy2)) / D); 01014 circomcenter.y = static_cast<float> (p1.y + (ptB.x*(cx2 + cy2) - ptC.x*(bx2 + by2)) / D); 01015 } 01016 01017 radius = 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)); 01018 } 01019 01020 /////////////////////////////////////////////////////////////////////////////////////////////// 01021 template<typename PointInT> inline void 01022 pcl::TextureMapping<PointInT>::getTriangleCircumcscribedCircleCentroid ( const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circumcenter, double &radius) 01023 { 01024 // compute centroid's coordinates (translate back to original coordinates) 01025 circumcenter.x = static_cast<float> (p1.x + p2.x + p3.x ) / 3; 01026 circumcenter.y = static_cast<float> (p1.y + p2.y + p3.y ) / 3; 01027 double r1 = (circumcenter.x - p1.x) * (circumcenter.x - p1.x) + (circumcenter.y - p1.y) * (circumcenter.y - p1.y) ; 01028 double r2 = (circumcenter.x - p2.x) * (circumcenter.x - p2.x) + (circumcenter.y - p2.y) * (circumcenter.y - p2.y) ; 01029 double r3 = (circumcenter.x - p3.x) * (circumcenter.x - p3.x) + (circumcenter.y - p3.y) * (circumcenter.y - p3.y) ; 01030 01031 // radius 01032 radius = std::sqrt( std::max( r1, std::max( r2, r3) )) ; 01033 } 01034 01035 01036 /////////////////////////////////////////////////////////////////////////////////////////////// 01037 template<typename PointInT> inline bool 01038 pcl::TextureMapping<PointInT>::getPointUVCoordinates(const pcl::PointXYZ &pt, const Camera &cam, pcl::PointXY &UV_coordinates) 01039 { 01040 if (pt.z > 0) 01041 { 01042 // compute image center and dimension 01043 double sizeX = cam.width; 01044 double sizeY = cam.height; 01045 double cx = sizeX / 2.0; 01046 double cy = sizeY / 2.0; 01047 01048 // project point on camera's image plane 01049 UV_coordinates.x = static_cast<float> ((cam.focal_length * (pt.x / pt.z) + cx) / sizeX); //horizontal 01050 UV_coordinates.y = 1.0f - static_cast<float> ((cam.focal_length * (pt.y / pt.z) + cy) / sizeY); //vertical 01051 01052 // point is visible! 01053 if (UV_coordinates.x >= 0.0 && UV_coordinates.x <= 1.0 && UV_coordinates.y >= 0.0 && UV_coordinates.y <= 1.0) 01054 return (true); // point was visible by the camera 01055 } 01056 01057 // point is NOT visible by the camera 01058 UV_coordinates.x = -1.0f; 01059 UV_coordinates.y = -1.0f; 01060 return (false); // point was not visible by the camera 01061 } 01062 01063 /////////////////////////////////////////////////////////////////////////////////////////////// 01064 template<typename PointInT> inline bool 01065 pcl::TextureMapping<PointInT>::checkPointInsideTriangle(const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, const pcl::PointXY &pt) 01066 { 01067 // Compute vectors 01068 Eigen::Vector2d v0, v1, v2; 01069 v0(0) = p3.x - p1.x; v0(1) = p3.y - p1.y; // v0= C - A 01070 v1(0) = p2.x - p1.x; v1(1) = p2.y - p1.y; // v1= B - A 01071 v2(0) = pt.x - p1.x; v2(1) = pt.y - p1.y; // v2= P - A 01072 01073 // Compute dot products 01074 double dot00 = v0.dot(v0); // dot00 = dot(v0, v0) 01075 double dot01 = v0.dot(v1); // dot01 = dot(v0, v1) 01076 double dot02 = v0.dot(v2); // dot02 = dot(v0, v2) 01077 double dot11 = v1.dot(v1); // dot11 = dot(v1, v1) 01078 double dot12 = v1.dot(v2); // dot12 = dot(v1, v2) 01079 01080 // Compute barycentric coordinates 01081 double invDenom = 1.0 / (dot00*dot11 - dot01*dot01); 01082 double u = (dot11*dot02 - dot01*dot12) * invDenom; 01083 double v = (dot00*dot12 - dot01*dot02) * invDenom; 01084 01085 // Check if point is in triangle 01086 return ((u >= 0) && (v >= 0) && (u + v < 1)); 01087 } 01088 01089 /////////////////////////////////////////////////////////////////////////////////////////////// 01090 template<typename PointInT> inline bool 01091 pcl::TextureMapping<PointInT>::isFaceProjected (const Camera &camera, const pcl::PointXYZ &p1, const pcl::PointXYZ &p2, const pcl::PointXYZ &p3, pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3) 01092 { 01093 return (getPointUVCoordinates(p1, camera, proj1) 01094 && 01095 getPointUVCoordinates(p2, camera, proj2) 01096 && 01097 getPointUVCoordinates(p3, camera, proj3) 01098 ); 01099 } 01100 01101 #define PCL_INSTANTIATE_TextureMapping(T) \ 01102 template class PCL_EXPORTS pcl::TextureMapping<T>; 01103 01104 #endif /* TEXTURE_MAPPING_HPP_ */ 01105