Point Cloud Library (PCL)
1.3.1
|
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: texture_mapping.hpp 1006 2011-07-13 13:07:00 ktran $ 00035 * 00036 */ 00038 #ifndef PCL_SURFACE_IMPL_TEXTURE_MAPPING_HPP_ 00039 #define PCL_SURFACE_IMPL_TEXTURE_MAPPING_HPP_ 00040 00041 #include "pcl/surface/texture_mapping.h" 00042 00044 template <typename PointInT> float 00045 pcl::TextureMapping<PointInT>::getDistance (Eigen::Vector3f &p1, Eigen::Vector3f &p2) 00046 { 00047 return std::sqrt((p1[0]-p2[0])*(p1[0]-p2[0]) + (p1[1]-p2[1])*(p1[1]-p2[1]) + (p1[2]-p2[2])*(p1[2]-p2[2])); 00048 } 00049 00051 template <typename PointInT> std::vector<Eigen::Vector2f> 00052 pcl::TextureMapping<PointInT>::mapTexture2Face (Eigen::Vector3f &p1, Eigen::Vector3f &p2, Eigen::Vector3f &p3) 00053 { 00054 std::vector<Eigen::Vector2f> tex_coordinates; 00055 // process for each face 00056 Eigen::Vector3f p1p2(p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]); 00057 Eigen::Vector3f p1p3(p3[0] - p1[0], p3[1] - p1[1], p3[2] - p1[2]); 00058 Eigen::Vector3f p2p3(p3[0] - p2[0], p3[1] - p2[1], p3[2] - p2[2]); 00059 00060 // Normalize 00061 p1p2 = p1p2/std::sqrt(p1p2.dot(p1p2)); 00062 p1p3 = p1p3/std::sqrt(p1p3.dot(p1p3)); 00063 p2p3 = p2p3/std::sqrt(p2p3.dot(p2p3)); 00064 00065 // compute vector normal of a face 00066 Eigen::Vector3f f_normal = p1p2.cross(p1p3); 00067 f_normal = f_normal/std::sqrt(f_normal.dot(f_normal)); 00068 00069 // project vector field onto the face: vector v1_projected = v1 - Dot(v1, n) * n; 00070 Eigen::Vector3f f_vector_field = vector_field_ - vector_field_.dot(f_normal) * f_normal; 00071 00072 // Normalize 00073 f_vector_field = f_vector_field/std::sqrt(f_vector_field.dot(f_vector_field)); 00074 00075 // texture coordinates 00076 Eigen::Vector2f tp1, tp2, tp3; 00077 00078 double alpha = std::acos(f_vector_field.dot(p1p2)); 00079 00080 // distance between 3 vertices of triangles 00081 double e1 = getDistance(p2, p3)/f_; 00082 double e2 = getDistance(p1, p3)/f_; 00083 double e3 = getDistance(p1, p2)/f_; 00084 00085 // initialize 00086 tp1[0] = 0.0; 00087 tp1[1] = 0.0; 00088 00089 tp2[0] = e3; 00090 tp2[1] = 0.0; 00091 00092 // determine texture coordinate tp3; 00093 double cos_p1 = (e2*e2+e3*e3-e1*e1)/(2*e2*e3); 00094 double sin_p1 = sqrt(1-(cos_p1*cos_p1)); 00095 00096 tp3[0] = cos_p1*e2; 00097 tp3[1] = sin_p1*e2; 00098 00099 // rotating by alpha (angle between V and pp1 & pp2) 00100 Eigen::Vector2f r_tp2, r_tp3; 00101 r_tp2[0] = tp2[0]*std::cos(alpha) - tp2[1]*std::sin(alpha); 00102 r_tp2[1] = tp2[0]*std::sin(alpha) + tp2[1]*std::cos(alpha); 00103 00104 r_tp3[0] = tp3[0]*std::cos(alpha) - tp3[1]*std::sin(alpha); 00105 r_tp3[1] = tp3[0]*std::sin(alpha) + tp3[1]*std::cos(alpha); 00106 00107 // shifting 00108 tp1[0] = tp1[0]; 00109 tp2[0] = r_tp2[0]; 00110 tp3[0] = r_tp3[0]; 00111 tp1[1] = tp1[1]; 00112 tp2[1] = r_tp2[1]; 00113 tp3[1] = r_tp3[1]; 00114 00115 float min_x = tp1[0]; 00116 float min_y = tp1[1]; 00117 if (min_x > tp2[0]) min_x = tp2[0]; 00118 if (min_x > tp3[0]) min_x = tp3[0]; 00119 if (min_y > tp2[1]) min_y = tp2[1]; 00120 if (min_y > tp3[1]) min_y = tp3[1]; 00121 00122 if(min_x < 0) 00123 { 00124 tp1[0] = tp1[0] - min_x; 00125 tp2[0] = tp2[0] - min_x; 00126 tp3[0] = tp3[0] - min_x; 00127 } 00128 if(min_y < 0) 00129 { 00130 tp1[1] = tp1[1] - min_y; 00131 tp2[1] = tp2[1] - min_y; 00132 tp3[1] = tp3[1] - min_y; 00133 } 00134 00135 tex_coordinates.push_back(tp1); 00136 tex_coordinates.push_back(tp2); 00137 tex_coordinates.push_back(tp3); 00138 return tex_coordinates; 00139 } 00140 00142 template <typename PointInT> void 00143 pcl::TextureMapping<PointInT>::mapTexture2Mesh (pcl::TextureMesh &tex_mesh) 00144 { 00145 // mesh information 00146 int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height; 00147 int point_size = tex_mesh.cloud.data.size () / nr_points; 00148 00149 // temporary PointXYZ 00150 float x, y, z; 00151 // temporary face 00152 Eigen::Vector3f facet[3]; 00153 00154 // texture coordinates for each mesh 00155 std::vector< std::vector<Eigen::Vector2f> > texture_map; 00156 00157 for (size_t m = 0; m < tex_mesh.tex_polygons.size(); ++m) 00158 { 00159 // texture coordinates for each mesh 00160 std::vector<Eigen::Vector2f> texture_map_tmp; 00161 00162 // processing for each face 00163 for (size_t i=0; i < tex_mesh.tex_polygons[m].size(); ++i) 00164 { 00165 size_t idx; 00166 00167 // get facet information 00168 for (size_t j=0; j < tex_mesh.tex_polygons[m][i].vertices.size(); ++j) 00169 { 00170 idx = tex_mesh.tex_polygons[m][i].vertices[j]; 00171 memcpy (&x, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset], sizeof (float)); 00172 memcpy (&y, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset], sizeof (float)); 00173 memcpy (&z, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset], sizeof (float)); 00174 facet[j][0] = x; 00175 facet[j][1] = y; 00176 facet[j][2] = z; 00177 } 00178 00179 // get texture coordinates of each face 00180 std::vector<Eigen::Vector2f> tex_coordinates = mapTexture2Face(facet[0], facet[1], facet[2]); 00181 for(size_t n = 0; n < tex_coordinates.size(); ++n) 00182 texture_map_tmp.push_back(tex_coordinates[n]); 00183 }// end faces 00184 00185 // texture materials 00186 std::stringstream tex_name; 00187 tex_name << "material_"<< m; 00188 tex_name >> tex_material_.tex_name; 00189 tex_material_.tex_file = tex_files_[m]; 00190 tex_mesh.tex_materials.push_back(tex_material_); 00191 00192 // texture coordinates 00193 tex_mesh.tex_coordinates.push_back(texture_map_tmp); 00194 }// end meshes 00195 } 00196 00198 template <typename PointInT> void 00199 pcl::TextureMapping<PointInT>::mapTexture2MeshUV (pcl::TextureMesh &tex_mesh){ 00200 00201 // mesh information 00202 int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height; 00203 int point_size = tex_mesh.cloud.data.size () / nr_points; 00204 00205 float x_lowest = 100000; 00206 float x_highest = 0 ; 00207 float y_lowest = 100000; 00208 //float y_highest = 0 ; 00209 float z_lowest = 100000; 00210 float z_highest = 0; 00211 float x_, y_, z_; 00212 00213 for (int i =0; i < nr_points; ++i) 00214 { 00215 memcpy (&x_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[0].offset], sizeof (float)); 00216 memcpy (&y_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[1].offset], sizeof (float)); 00217 memcpy (&z_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[2].offset], sizeof (float)); 00218 // x 00219 if (x_ <= x_lowest) x_lowest = x_; 00220 if (x_ > x_lowest) x_highest = x_; 00221 00222 // y 00223 if (y_ <= y_lowest) y_lowest = y_; 00224 //if (y_ > y_lowest) y_highest = y_; 00225 00226 // z 00227 if (z_ <= z_lowest) z_lowest = z_; 00228 if (z_ > z_lowest) z_highest = z_; 00229 } 00230 // x 00231 float x_range = (x_lowest - x_highest)*-1; 00232 float x_offset = 0 - x_lowest; 00233 // x 00234 //float y_range = (y_lowest - y_highest)*-1; 00235 //float y_offset = 0 - y_lowest; 00236 // z 00237 float z_range = (z_lowest - z_highest)*-1; 00238 float z_offset = 0 - z_lowest; 00239 00240 00241 // texture coordinates for each mesh 00242 std::vector< std::vector<Eigen::Vector2f> > texture_map; 00243 00244 for (size_t m = 0; m < tex_mesh.tex_polygons.size(); ++m) 00245 { 00246 // texture coordinates for each mesh 00247 std::vector<Eigen::Vector2f> texture_map_tmp; 00248 00249 // processing for each face 00250 for (size_t i=0; i < tex_mesh.tex_polygons[m].size(); ++i) 00251 { 00252 size_t idx; 00253 Eigen::Vector2f tmp_VT; 00254 for (size_t j=0; j < tex_mesh.tex_polygons[m][i].vertices.size(); ++j) 00255 { 00256 idx = tex_mesh.tex_polygons[m][i].vertices[j]; 00257 memcpy (&x_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset], sizeof (float)); 00258 memcpy (&y_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset], sizeof (float)); 00259 memcpy (&z_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset], sizeof (float)); 00260 00261 // calculate uv coordinates 00262 tmp_VT[0] = (x_ + x_offset)/x_range; 00263 tmp_VT[1] = (z_ + z_offset)/z_range; 00264 texture_map_tmp.push_back(tmp_VT); 00265 } 00266 }// end faces 00267 00268 // texture materials 00269 std::stringstream tex_name; 00270 tex_name << "material_"<< m; 00271 tex_name >> tex_material_.tex_name; 00272 tex_material_.tex_file = tex_files_[m]; 00273 tex_mesh.tex_materials.push_back(tex_material_); 00274 00275 // texture coordinates 00276 tex_mesh.tex_coordinates.push_back(texture_map_tmp); 00277 }// end meshes 00278 } 00279 00280 #define PCL_INSTANTIATE_TextureMapping(T) \ 00281 template class PCL_EXPORTS pcl::TextureMapping<T>; 00282 00283 #endif /* TEXTURE_MAPPING_HPP_ */ 00284