texture_mapping.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, Willow Garage, Inc.
 *
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 *
 */
 
#ifndef PCL_SURFACE_TEXTURE_MAPPING_H_
#define PCL_SURFACE_TEXTURE_MAPPING_H_
 
#include <pcl/surface/reconstruction.h>
#include <pcl/common/transforms.h>
#include <pcl/TextureMesh.h>
#include <pcl/octree/octree.h>
#include <rtabmap/core/ProgressState.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
 
 
namespace pcl
{
  namespace texture_mapping
  {
        
    struct Camera
    {
      Camera () : pose (), focal_length (), focal_length_w (-1), focal_length_h (-1),
        center_w (-1), center_h (-1), height (), width (), texture_file () {}
      Eigen::Affine3f pose;
      double focal_length;
      double focal_length_w;  // optional
      double focal_length_h;  // optinoal
      double center_w;  // optional
      double center_h;  // optional
      double height;
      double width;
      std::string texture_file;
      std::vector<double> roi; // [x, y, width, height]
      cv::Mat depth;
 
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
    struct UvIndex
    {
      UvIndex () : idx_cloud (), idx_face () {}
      int idx_cloud; // Index of the PointXYZ in the camera's cloud
      int idx_face; // Face corresponding to that projection
    };
    
    typedef std::vector<Camera, Eigen::aligned_allocator<Camera> > CameraVector;
    
  }
  
  template<typename PointInT>
  class TextureMapping
  {
    public:
     
      typedef boost::shared_ptr< TextureMapping < PointInT > > Ptr;
      typedef boost::shared_ptr< const TextureMapping < PointInT > > ConstPtr;
 
      typedef pcl::PointCloud<PointInT> PointCloud;
      typedef typename PointCloud::Ptr PointCloudPtr;
      typedef typename PointCloud::ConstPtr PointCloudConstPtr;
 
      typedef pcl::octree::OctreePointCloudSearch<PointInT> Octree;
      typedef typename Octree::Ptr OctreePtr;
      typedef typename Octree::ConstPtr OctreeConstPtr;
      
      typedef pcl::texture_mapping::Camera Camera;
      typedef pcl::texture_mapping::UvIndex UvIndex;
 
      TextureMapping () :
        f_ (), vector_field_ (), tex_files_ (), tex_material_ (), max_distance_(0.0f), max_depth_error_(0.0f), max_angle_(0.0f), min_cluster_size_(50)
      {
      }
 
      ~TextureMapping ()
      {
      }
 
      inline void
      setF (float f)
      {
        f_ = f;
      }
 
      inline void
      setVectorField (float x, float y, float z)
      {
        vector_field_ = Eigen::Vector3f (x, y, z);
        // normalize vector field
        vector_field_ = vector_field_ / std::sqrt (vector_field_.dot (vector_field_));
      }
 
      inline void
      setTextureFiles (std::vector<std::string> tex_files)
      {
        tex_files_ = tex_files;
      }
 
      inline void
      setTextureMaterials (TexMaterial tex_material)
      {
        tex_material_ = tex_material;
      }
 
      inline void
          setMaxDistance(float maxDistance)
      {
          max_distance_ = maxDistance;
      }
 
      inline void
          setMaxDepthError(float maxDepthError)
          {
          max_depth_error_ = maxDepthError;
          }
 
      inline void
          setMaxAngle(float maxAngle)
          {
          max_angle_ = maxAngle;
          }
 
      inline void
          setMinClusterSize(int size)
      {
          min_cluster_size_ = size;
      }
 
      void
      mapTexture2Mesh (pcl::TextureMesh &tex_mesh);
 
      void
      mapTexture2MeshUV (pcl::TextureMesh &tex_mesh);
 
      void
      mapMultipleTexturesToMeshUV (pcl::TextureMesh &tex_mesh, 
                                   pcl::texture_mapping::CameraVector &cams);
 
      inline bool
      getPointUVCoordinates (const PointInT &pt, const Camera &cam, Eigen::Vector2f &UV_coordinates)
      {
        // if the point is in front of the camera
        if (pt.z > 0 && (max_distance_ <= 0.0f || pt.z < max_distance_))
        {
          // compute image center and dimension
          double sizeX = cam.width;
          double sizeY = cam.height;
          double cx, cy;
          if (cam.center_w > 0)
            cx = cam.center_w;
          else
            cx = (sizeX) / 2.0;
          if (cam.center_h > 0)
            cy = cam.center_h;
          else
            cy = (sizeY) / 2.0;
 
          double focal_x, focal_y;
          if (cam.focal_length_w > 0)
            focal_x = cam.focal_length_w;
          else
            focal_x = cam.focal_length;
          if (cam.focal_length_h>0)
            focal_y = cam.focal_length_h;
          else
            focal_y = cam.focal_length;
 
          // project point on image frame
          UV_coordinates[0] = static_cast<float> ((focal_x * (pt.x / pt.z) + cx) / sizeX); //horizontal
          UV_coordinates[1] = 1.0f - static_cast<float> (((focal_y * (pt.y / pt.z) + cy) / sizeY)); //vertical
 
          // point is visible!
          if (UV_coordinates[0] >= 0.0 && UV_coordinates[0] <= 1.0 && UV_coordinates[1] >= 0.0 && UV_coordinates[1]
                                                                                                                 <= 1.0)
            return (true);
        }
 
        // point is NOT visible by the camera
        UV_coordinates[0] = -1.0;
        UV_coordinates[1] = -1.0;
        return (false);
      }
 
      inline bool
      isPointOccluded (const PointInT &pt, const OctreePtr octree);
 
      void
      removeOccludedPoints (const PointCloudPtr &input_cloud,
                            PointCloudPtr &filtered_cloud, const double octree_voxel_size,
                            std::vector<int> &visible_indices, std::vector<int> &occluded_indices);
 
      void
      removeOccludedPoints (const pcl::TextureMesh &tex_mesh, pcl::TextureMesh &cleaned_mesh, const double octree_voxel_size);
 
 
      void
      removeOccludedPoints (const pcl::TextureMesh &tex_mesh, PointCloudPtr &filtered_cloud, const double octree_voxel_size);
 
 
      int
      sortFacesByCamera (pcl::TextureMesh &tex_mesh, 
                         pcl::TextureMesh &sorted_mesh, 
                         const pcl::texture_mapping::CameraVector &cameras,
                         const double octree_voxel_size, PointCloud &visible_pts);
 
      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);
 
      void
      showOcclusions (pcl::TextureMesh &tex_mesh, 
                      pcl::PointCloud<pcl::PointXYZI>::Ptr &colored_cloud,
                      double octree_voxel_size, 
                      bool show_nb_occlusions = true, 
                      int max_occlusions = 4);
 
      void 
      textureMeshwithMultipleCameras (pcl::TextureMesh &mesh, 
                                      const pcl::texture_mapping::CameraVector &cameras);
      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);
 
    protected:
      float f_;
 
      Eigen::Vector3f vector_field_;
 
      std::vector<std::string> tex_files_;
 
      TexMaterial tex_material_;
 
      float max_distance_;
 
      float max_depth_error_;
 
      float max_angle_;
 
      int min_cluster_size_;
 
      std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >
      mapTexture2Face (const Eigen::Vector3f &p1, const Eigen::Vector3f &p2, const Eigen::Vector3f &p3);
 
      inline void
      getTriangleCircumcenterAndSize (const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circumcenter, double &radius);
 
      
      inline void 
      getTriangleCircumcscribedCircleCentroid ( const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circumcenter, double &radius);
 
 
      inline bool
      getPointUVCoordinates (const PointInT &pt, const Camera &cam, pcl::PointXY &UV_coordinates);
 
      inline bool
      isFaceProjected (const Camera &camera, 
                       const PointInT &p1, const PointInT &p2, const PointInT &p3, 
                       pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3);
 
      inline bool
      checkPointInsideTriangle (const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, const pcl::PointXY &pt);
 
      std::string
      getClassName () const
      {
        return ("TextureMapping");
      }
 
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };
}
 
#include <pcl18/surface/impl/texture_mapping.hpp>
 
#endif /* TEXTURE_MAPPING_H_ */