Point Cloud Library (PCL)  1.3.1
texture_mapping.hpp
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines