texture_mapping.hpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, 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_IMPL_TEXTURE_MAPPING_HPP_
#define PCL_SURFACE_IMPL_TEXTURE_MAPPING_HPP_

#include <pcl/common/distances.h>
#include <pcl18/surface/texture_mapping.h>
#include <pcl/search/octree.h>

template<typename PointInT> std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >
pcl::TextureMapping<PointInT>::mapTexture2Face (
    const Eigen::Vector3f &p1, 
    const Eigen::Vector3f &p2, 
    const Eigen::Vector3f &p3)
{
  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > tex_coordinates;
  // process for each face
  Eigen::Vector3f p1p2 (p2[0] - p1[0], p2[1] - p1[1], p2[2] - p1[2]);
  Eigen::Vector3f p1p3 (p3[0] - p1[0], p3[1] - p1[1], p3[2] - p1[2]);
  Eigen::Vector3f p2p3 (p3[0] - p2[0], p3[1] - p2[1], p3[2] - p2[2]);

  // Normalize
  p1p2 = p1p2 / std::sqrt (p1p2.dot (p1p2));
  p1p3 = p1p3 / std::sqrt (p1p3.dot (p1p3));
  p2p3 = p2p3 / std::sqrt (p2p3.dot (p2p3));

  // compute vector normal of a face
  Eigen::Vector3f f_normal = p1p2.cross (p1p3);
  f_normal = f_normal / std::sqrt (f_normal.dot (f_normal));

  // project vector field onto the face: vector v1_projected = v1 - Dot(v1, n) * n;
  Eigen::Vector3f f_vector_field = vector_field_ - vector_field_.dot (f_normal) * f_normal;

  // Normalize
  f_vector_field = f_vector_field / std::sqrt (f_vector_field.dot (f_vector_field));

  // texture coordinates
  Eigen::Vector2f tp1, tp2, tp3;

  double alpha = std::acos (f_vector_field.dot (p1p2));

  // distance between 3 vertices of triangles
  double e1 = (p2 - p3).norm () / f_;
  double e2 = (p1 - p3).norm () / f_;
  double e3 = (p1 - p2).norm () / f_;

  // initialize
  tp1[0] = 0.0;
  tp1[1] = 0.0;

  tp2[0] = static_cast<float> (e3);
  tp2[1] = 0.0;

  // determine texture coordinate tp3;
  double cos_p1 = (e2 * e2 + e3 * e3 - e1 * e1) / (2 * e2 * e3);
  double sin_p1 = sqrt (1 - (cos_p1 * cos_p1));

  tp3[0] = static_cast<float> (cos_p1 * e2);
  tp3[1] = static_cast<float> (sin_p1 * e2);

  // rotating by alpha (angle between V and pp1 & pp2)
  Eigen::Vector2f r_tp2, r_tp3;
  r_tp2[0] = static_cast<float> (tp2[0] * std::cos (alpha) - tp2[1] * std::sin (alpha));
  r_tp2[1] = static_cast<float> (tp2[0] * std::sin (alpha) + tp2[1] * std::cos (alpha));

  r_tp3[0] = static_cast<float> (tp3[0] * std::cos (alpha) - tp3[1] * std::sin (alpha));
  r_tp3[1] = static_cast<float> (tp3[0] * std::sin (alpha) + tp3[1] * std::cos (alpha));

  // shifting
  tp1[0] = tp1[0];
  tp2[0] = r_tp2[0];
  tp3[0] = r_tp3[0];
  tp1[1] = tp1[1];
  tp2[1] = r_tp2[1];
  tp3[1] = r_tp3[1];

  float min_x = tp1[0];
  float min_y = tp1[1];
  if (min_x > tp2[0])
    min_x = tp2[0];
  if (min_x > tp3[0])
    min_x = tp3[0];
  if (min_y > tp2[1])
    min_y = tp2[1];
  if (min_y > tp3[1])
    min_y = tp3[1];

  if (min_x < 0)
  {
    tp1[0] = tp1[0] - min_x;
    tp2[0] = tp2[0] - min_x;
    tp3[0] = tp3[0] - min_x;
  }
  if (min_y < 0)
  {
    tp1[1] = tp1[1] - min_y;
    tp2[1] = tp2[1] - min_y;
    tp3[1] = tp3[1] - min_y;
  }

  tex_coordinates.push_back (tp1);
  tex_coordinates.push_back (tp2);
  tex_coordinates.push_back (tp3);
  return (tex_coordinates);
}

template<typename PointInT> void
pcl::TextureMapping<PointInT>::mapTexture2Mesh (pcl::TextureMesh &tex_mesh)
{
  // mesh information
  int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height;
  int point_size = static_cast<int> (tex_mesh.cloud.data.size ()) / nr_points;

  // temporary PointXYZ
  float x, y, z;
  // temporary face
  Eigen::Vector3f facet[3];

  // texture coordinates for each mesh
  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >texture_map;

  for (size_t m = 0; m < tex_mesh.tex_polygons.size (); ++m)
  {
    // texture coordinates for each mesh
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
        std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
#else
        std::vector<Eigen::Vector2f> texture_map_tmp;
#endif

    // processing for each face
    for (size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i)
    {
      size_t idx;

      // get facet information
      for (size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
      {
        idx = tex_mesh.tex_polygons[m][i].vertices[j];
        memcpy (&x, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float));
        memcpy (&y, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float));
        memcpy (&z, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float));
        facet[j][0] = x;
        facet[j][1] = y;
        facet[j][2] = z;
      }

      // get texture coordinates of each face
      std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > tex_coordinates = mapTexture2Face (facet[0], facet[1], facet[2]);
      for (size_t n = 0; n < tex_coordinates.size (); ++n)
        texture_map_tmp.push_back (tex_coordinates[n]);
    }// end faces

    // texture materials
    std::stringstream tex_name;
    tex_name << "material_" << m;
    tex_name >> tex_material_.tex_name;
    tex_material_.tex_file = tex_files_[m];
    tex_mesh.tex_materials.push_back (tex_material_);

    // texture coordinates
    tex_mesh.tex_coordinates.push_back (texture_map_tmp);
  }// end meshes
}

template<typename PointInT> void
pcl::TextureMapping<PointInT>::mapTexture2MeshUV (pcl::TextureMesh &tex_mesh)
{
  // mesh information
  int nr_points = tex_mesh.cloud.width * tex_mesh.cloud.height;
  int point_size = static_cast<int> (tex_mesh.cloud.data.size ()) / nr_points;

  float x_lowest = 100000;
  float x_highest = 0;
  float y_lowest = 100000;
  //float y_highest = 0 ;
  float z_lowest = 100000;
  float z_highest = 0;
  float x_, y_, z_;

  for (int i = 0; i < nr_points; ++i)
  {
    memcpy (&x_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float));
    memcpy (&y_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float));
    memcpy (&z_, &tex_mesh.cloud.data[i * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float));
    // x
    if (x_ <= x_lowest)
      x_lowest = x_;
    if (x_ > x_lowest)
      x_highest = x_;

    // y
    if (y_ <= y_lowest)
      y_lowest = y_;
    //if (y_ > y_lowest) y_highest = y_;

    // z
    if (z_ <= z_lowest)
      z_lowest = z_;
    if (z_ > z_lowest)
      z_highest = z_;
  }
  // x
  float x_range = (x_lowest - x_highest) * -1;
  float x_offset = 0 - x_lowest;
  // x
  // float y_range = (y_lowest - y_highest)*-1;
  // float y_offset = 0 - y_lowest;
  // z
  float z_range = (z_lowest - z_highest) * -1;
  float z_offset = 0 - z_lowest;

  // texture coordinates for each mesh
  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >texture_map;

  for (size_t m = 0; m < tex_mesh.tex_polygons.size (); ++m)
  {
    // texture coordinates for each mesh
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
        std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
#else
        std::vector<Eigen::Vector2f> texture_map_tmp;
#endif

    // processing for each face
    for (size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i)
    {
      size_t idx;
      Eigen::Vector2f tmp_VT;
      for (size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
      {
        idx = tex_mesh.tex_polygons[m][i].vertices[j];
        memcpy (&x_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[0].offset], sizeof(float));
        memcpy (&y_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[1].offset], sizeof(float));
        memcpy (&z_, &tex_mesh.cloud.data[idx * point_size + tex_mesh.cloud.fields[2].offset], sizeof(float));

        // calculate uv coordinates
        tmp_VT[0] = (x_ + x_offset) / x_range;
        tmp_VT[1] = (z_ + z_offset) / z_range;
        texture_map_tmp.push_back (tmp_VT);
      }
    }// end faces

    // texture materials
    std::stringstream tex_name;
    tex_name << "material_" << m;
    tex_name >> tex_material_.tex_name;
    tex_material_.tex_file = tex_files_[m];
    tex_mesh.tex_materials.push_back (tex_material_);

    // texture coordinates
    tex_mesh.tex_coordinates.push_back (texture_map_tmp);
  }// end meshes
}

template<typename PointInT> void
pcl::TextureMapping<PointInT>::mapMultipleTexturesToMeshUV (pcl::TextureMesh &tex_mesh, pcl::texture_mapping::CameraVector &cams)
{

  if (tex_mesh.tex_polygons.size () != cams.size () + 1)
  {
    PCL_ERROR ("The mesh should be divided into nbCamera+1 sub-meshes.\n");
    PCL_ERROR ("You provided %d cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.tex_polygons.size ());
    return;
  }

  PCL_INFO ("You provided %d  cameras and a mesh containing %d sub-meshes.\n", cams.size (), tex_mesh.tex_polygons.size ());

  typename pcl::PointCloud<PointInT>::Ptr originalCloud (new pcl::PointCloud<PointInT>);
  typename pcl::PointCloud<PointInT>::Ptr camera_transformed_cloud (new pcl::PointCloud<PointInT>);

  // convert mesh's cloud to pcl format for ease
  pcl::fromPCLPointCloud2 (tex_mesh.cloud, *originalCloud);

  // texture coordinates for each mesh
  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texture_map;

  for (size_t m = 0; m < cams.size (); ++m)
  {
    // get current camera parameters
    Camera current_cam = cams[m];

    // get camera transform
    Eigen::Affine3f cam_trans = current_cam.pose;

    // transform cloud into current camera frame
    pcl::transformPointCloud (*originalCloud, *camera_transformed_cloud, cam_trans.inverse ());

    // vector of texture coordinates for each face
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
        std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
#else
        std::vector<Eigen::Vector2f> texture_map_tmp;
#endif

    // processing each face visible by this camera
    PointInT pt;
    size_t idx;
    for (size_t i = 0; i < tex_mesh.tex_polygons[m].size (); ++i)
    {
      Eigen::Vector2f tmp_VT;
      // for each point of this face
      for (size_t j = 0; j < tex_mesh.tex_polygons[m][i].vertices.size (); ++j)
      {
        // get point
        idx = tex_mesh.tex_polygons[m][i].vertices[j];
        pt = camera_transformed_cloud->points[idx];

        // compute UV coordinates for this point
        getPointUVCoordinates (pt, current_cam, tmp_VT);
        texture_map_tmp.push_back (tmp_VT);

      }// end points
    }// end faces

    // texture materials
    std::stringstream tex_name;
    tex_name << "material_" << m;
    tex_name >> tex_material_.tex_name;
    tex_material_.tex_file = current_cam.texture_file;
    tex_mesh.tex_materials.push_back (tex_material_);

    // texture coordinates
    tex_mesh.tex_coordinates.push_back (texture_map_tmp);
  }// end cameras

  // push on extra empty UV map (for unseen faces) so that obj writer does not crash!
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
        std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texture_map_tmp;
#else
        std::vector<Eigen::Vector2f> texture_map_tmp;
#endif
  for (size_t i = 0; i < tex_mesh.tex_polygons[cams.size ()].size (); ++i)
    for (size_t j = 0; j < tex_mesh.tex_polygons[cams.size ()][i].vertices.size (); ++j)
    {
      Eigen::Vector2f tmp_VT;
      tmp_VT[0] = -1;
      tmp_VT[1] = -1;
      texture_map_tmp.push_back (tmp_VT);
    }

  tex_mesh.tex_coordinates.push_back (texture_map_tmp);

  // push on an extra dummy material for the same reason
  std::stringstream tex_name;
  tex_name << "material_" << cams.size ();
  tex_name >> tex_material_.tex_name;
  tex_material_.tex_file = "occluded.jpg";
  tex_mesh.tex_materials.push_back (tex_material_);

}

template<typename PointInT> bool
pcl::TextureMapping<PointInT>::isPointOccluded (const PointInT &pt, OctreePtr octree)
{
  Eigen::Vector3f direction;
  direction (0) = pt.x;
  direction (1) = pt.y;
  direction (2) = pt.z;

  std::vector<int> indices;

  PointCloudConstPtr cloud (new PointCloud());
  cloud = octree->getInputCloud();

  double distance_threshold = octree->getResolution();

  // raytrace
  octree->getIntersectedVoxelIndices(direction, -direction, indices);

  int nbocc = static_cast<int> (indices.size ());
  for (size_t j = 0; j < indices.size (); j++)
  {
   // if intersected point is on the over side of the camera
   if (pt.z * cloud->points[indices[j]].z < 0)
   {
     nbocc--;
     continue;
   }

   if (fabs (cloud->points[indices[j]].z - pt.z) <= distance_threshold)
   {
     // points are very close to each-other, we do not consider the occlusion
     nbocc--;
   }
  }

  if (nbocc == 0)
   return (false);
  else
   return (true);
}

template<typename PointInT> void
pcl::TextureMapping<PointInT>::removeOccludedPoints (const PointCloudPtr &input_cloud,
                                                     PointCloudPtr &filtered_cloud,
                                                     const double octree_voxel_size, std::vector<int> &visible_indices,
                                                     std::vector<int> &occluded_indices)
{
  // variable used to filter occluded points by depth
  double maxDeltaZ = octree_voxel_size;

  // create an octree to perform rayTracing
  OctreePtr octree (new Octree (octree_voxel_size));
  // create octree structure
  octree->setInputCloud (input_cloud);
  // update bounding box automatically
  octree->defineBoundingBox ();
  // add points in the tree
  octree->addPointsFromInputCloud ();

  visible_indices.clear ();

  // for each point of the cloud, raycast toward camera and check intersected voxels.
  Eigen::Vector3f direction;
  std::vector<int> indices;
  for (size_t i = 0; i < input_cloud->points.size (); ++i)
  {
    direction (0) = input_cloud->points[i].x;
    direction (1) = input_cloud->points[i].y;
    direction (2) = input_cloud->points[i].z;

    // if point is not occluded
    octree->getIntersectedVoxelIndices (direction, -direction, indices);

    int nbocc = static_cast<int> (indices.size ());
    for (size_t j = 0; j < indices.size (); j++)
    {
      // if intersected point is on the over side of the camera
      if (input_cloud->points[i].z * input_cloud->points[indices[j]].z < 0)
      {
        nbocc--;
        continue;
      }

      if (fabs (input_cloud->points[indices[j]].z - input_cloud->points[i].z) <= maxDeltaZ)
      {
        // points are very close to each-other, we do not consider the occlusion
        nbocc--;
      }
    }

    if (nbocc == 0)
    {
      // point is added in the filtered mesh
      filtered_cloud->points.push_back (input_cloud->points[i]);
      visible_indices.push_back (static_cast<int> (i));
    }
    else
    {
      occluded_indices.push_back (static_cast<int> (i));
    }
  }

}

template<typename PointInT> void
pcl::TextureMapping<PointInT>::removeOccludedPoints (const pcl::TextureMesh &tex_mesh, pcl::TextureMesh &cleaned_mesh, const double octree_voxel_size)
{
  // copy mesh
  cleaned_mesh = tex_mesh;

  typename pcl::PointCloud<PointInT>::Ptr cloud (new pcl::PointCloud<PointInT>);
  typename pcl::PointCloud<PointInT>::Ptr filtered_cloud (new pcl::PointCloud<PointInT>);

  // load points into a PCL format
  pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);

  std::vector<int> visible, occluded;
  removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded);

  // Now that we know which points are visible, let's iterate over each face.
  // if the face has one invisible point => out!
  for (size_t polygons = 0; polygons < cleaned_mesh.tex_polygons.size (); ++polygons)
  {
    // remove all faces from cleaned mesh
    cleaned_mesh.tex_polygons[polygons].clear ();
    // iterate over faces
    for (size_t faces = 0; faces < tex_mesh.tex_polygons[polygons].size (); ++faces)
    {
      // check if all the face's points are visible
      bool faceIsVisible = true;
      std::vector<int>::iterator it;

      // iterate over face's vertex
      for (size_t points = 0; points < tex_mesh.tex_polygons[polygons][faces].vertices.size (); ++points)
      {
        it = find (occluded.begin (), occluded.end (), tex_mesh.tex_polygons[polygons][faces].vertices[points]);

        if (it == occluded.end ())
        {
          // point is not in the occluded vector
          // PCL_INFO ("  VISIBLE!\n");
        }
        else
        {
          // point was occluded
          // PCL_INFO("  OCCLUDED!\n");
          faceIsVisible = false;
        }
      }

      if (faceIsVisible)
      {
        cleaned_mesh.tex_polygons[polygons].push_back (tex_mesh.tex_polygons[polygons][faces]);
      }

    }
  }
}

template<typename PointInT> void
pcl::TextureMapping<PointInT>::removeOccludedPoints (const pcl::TextureMesh &tex_mesh, PointCloudPtr &filtered_cloud,
                      const double octree_voxel_size)
{
  PointCloudPtr cloud (new PointCloud);

  // load points into a PCL format
  pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);

  std::vector<int> visible, occluded;
  removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded);

}

template<typename PointInT> int
pcl::TextureMapping<PointInT>::sortFacesByCamera (pcl::TextureMesh &tex_mesh, pcl::TextureMesh &sorted_mesh,
                                                  const pcl::texture_mapping::CameraVector &cameras, const double octree_voxel_size,
                                                  PointCloud &visible_pts)
{
  if (tex_mesh.tex_polygons.size () != 1)
  {
    PCL_ERROR ("The mesh must contain only 1 sub-mesh!\n");
    return (-1);
  }

  if (cameras.size () == 0)
  {
    PCL_ERROR ("Must provide at least one camera info!\n");
    return (-1);
  }

  // copy mesh
  sorted_mesh = tex_mesh;
  // clear polygons from cleaned_mesh
  sorted_mesh.tex_polygons.clear ();

  typename pcl::PointCloud<PointInT>::Ptr original_cloud (new pcl::PointCloud<PointInT>);
  typename pcl::PointCloud<PointInT>::Ptr transformed_cloud (new pcl::PointCloud<PointInT>);
  typename pcl::PointCloud<PointInT>::Ptr filtered_cloud (new pcl::PointCloud<PointInT>);

  // load points into a PCL format
  pcl::fromPCLPointCloud2 (tex_mesh.cloud, *original_cloud);

  // for each camera
  for (size_t cam = 0; cam < cameras.size (); ++cam)
  {
    // get camera pose as transform
    Eigen::Affine3f cam_trans = cameras[cam].pose;

    // transform original cloud in camera coordinates
    pcl::transformPointCloud (*original_cloud, *transformed_cloud, cam_trans.inverse ());

    // find occlusions on transformed cloud
    std::vector<int> visible, occluded;
    removeOccludedPoints (transformed_cloud, filtered_cloud, octree_voxel_size, visible, occluded);
    visible_pts = *filtered_cloud;

    // find visible faces => add them to polygon N for camera N
    // add polygon group for current camera in clean
    std::vector<pcl::Vertices> visibleFaces_currentCam;
    // iterate over the faces of the current mesh
    for (size_t faces = 0; faces < tex_mesh.tex_polygons[0].size (); ++faces)
    {
      // check if all the face's points are visible
      bool faceIsVisible = true;
      std::vector<int>::iterator it;

      // iterate over face's vertex
      for (size_t current_pt_indice = 0; faceIsVisible && current_pt_indice < tex_mesh.tex_polygons[0][faces].vertices.size (); ++current_pt_indice)
      {
        // TODO this is far too long! Better create an helper function that raycasts here.
        it = find (occluded.begin (), occluded.end (), tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]);

        if (it == occluded.end ())
        {
          // point is not occluded
          // does it land on the camera's image plane?
          PointInT pt = transformed_cloud->points[tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]];
          Eigen::Vector2f dummy_UV;
          if (!getPointUVCoordinates (pt, cameras[cam], dummy_UV))
          {
            // point is not visible by the camera
            faceIsVisible = false;
          }
        }
        else
        {
          faceIsVisible = false;
        }
      }

      if (faceIsVisible)
      {
        // push current visible face into the sorted mesh
        visibleFaces_currentCam.push_back (tex_mesh.tex_polygons[0][faces]);
        // remove it from the unsorted mesh
        tex_mesh.tex_polygons[0].erase (tex_mesh.tex_polygons[0].begin () + faces);
        faces--;
      }

    }
    sorted_mesh.tex_polygons.push_back (visibleFaces_currentCam);
  }

  // we should only have occluded and non-visible faces left in tex_mesh.tex_polygons[0]
  // we need to add them as an extra polygon in the sorted mesh
  sorted_mesh.tex_polygons.push_back (tex_mesh.tex_polygons[0]);
  return (0);
}

template<typename PointInT> void
pcl::TextureMapping<PointInT>::showOcclusions (const PointCloudPtr &input_cloud,
                                               pcl::PointCloud<pcl::PointXYZI>::Ptr &colored_cloud,
                                               const double octree_voxel_size, const bool show_nb_occlusions,
                                               const int max_occlusions)
                                               {
  // variable used to filter occluded points by depth
  double maxDeltaZ = octree_voxel_size * 2.0;

  // create an octree to perform rayTracing
  pcl::octree::OctreePointCloudSearch<PointInT> *octree;
  octree = new pcl::octree::OctreePointCloudSearch<PointInT> (octree_voxel_size);
  // create octree structure
  octree->setInputCloud (input_cloud);
  // update bounding box automatically
  octree->defineBoundingBox ();
  // add points in the tree
  octree->addPointsFromInputCloud ();

  // ray direction
  Eigen::Vector3f direction;

  std::vector<int> indices;
  // point from where we ray-trace
  pcl::PointXYZI pt;

  std::vector<double> zDist;
  std::vector<double> ptDist;
  // for each point of the cloud, ray-trace toward the camera and check intersected voxels.
  for (size_t i = 0; i < input_cloud->points.size (); ++i)
  {
    direction (0) = input_cloud->points[i].x;
    pt.x = input_cloud->points[i].x;
    direction (1) = input_cloud->points[i].y;
    pt.y = input_cloud->points[i].y;
    direction (2) = input_cloud->points[i].z;
    pt.z = input_cloud->points[i].z;

    // get number of occlusions for that point
    indices.clear ();
    int nbocc = octree->getIntersectedVoxelIndices (direction, -direction, indices);

    nbocc = static_cast<int> (indices.size ());

    // TODO need to clean this up and find tricks to get remove aliasaing effect on planes
    for (size_t j = 0; j < indices.size (); j++)
    {
      // if intersected point is on the over side of the camera
      if (pt.z * input_cloud->points[indices[j]].z < 0)
      {
        nbocc--;
      }
      else if (fabs (input_cloud->points[indices[j]].z - pt.z) <= maxDeltaZ)
      {
        // points are very close to each-other, we do not consider the occlusion
        nbocc--;
      }
      else
      {
        zDist.push_back (fabs (input_cloud->points[indices[j]].z - pt.z));
        ptDist.push_back (pcl::euclideanDistance (input_cloud->points[indices[j]], pt));
      }
    }

    if (show_nb_occlusions)
      (nbocc <= max_occlusions) ? (pt.intensity = static_cast<float> (nbocc)) : (pt.intensity = static_cast<float> (max_occlusions));
    else
      (nbocc == 0) ? (pt.intensity = 0) : (pt.intensity = 1);

    colored_cloud->points.push_back (pt);
  }

  if (zDist.size () >= 2)
  {
    std::sort (zDist.begin (), zDist.end ());
    std::sort (ptDist.begin (), ptDist.end ());
  }
}

template<typename PointInT> void
pcl::TextureMapping<PointInT>::showOcclusions (pcl::TextureMesh &tex_mesh, pcl::PointCloud<pcl::PointXYZI>::Ptr &colored_cloud,
                  double octree_voxel_size, bool show_nb_occlusions, int max_occlusions)
{
  // load points into a PCL format
  typename pcl::PointCloud<PointInT>::Ptr cloud (new pcl::PointCloud<PointInT>);
  pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);

  showOcclusions (cloud, colored_cloud, octree_voxel_size, show_nb_occlusions, max_occlusions);
}

template<typename PointInT> void
pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras (pcl::TextureMesh &mesh, const pcl::texture_mapping::CameraVector &cameras)
{

  if (mesh.tex_polygons.size () != 1)
    return;

  typename pcl::PointCloud<PointInT>::Ptr mesh_cloud (new pcl::PointCloud<PointInT>);

  pcl::fromPCLPointCloud2 (mesh.cloud, *mesh_cloud);

  std::vector<pcl::Vertices> faces;

  for (int current_cam = 0; current_cam < static_cast<int> (cameras.size ()); ++current_cam)
  {
    PCL_INFO ("Processing camera %d of %d.\n", current_cam+1, cameras.size ());
    
    // transform mesh into camera's frame
    typename pcl::PointCloud<PointInT>::Ptr camera_cloud (new pcl::PointCloud<PointInT>);
    pcl::transformPointCloud (*mesh_cloud, *camera_cloud, cameras[current_cam].pose.inverse ());

    // CREATE UV MAP FOR CURRENT FACES
    pcl::PointCloud<pcl::PointXY>::Ptr projections (new pcl::PointCloud<pcl::PointXY>);
    std::vector<pcl::Vertices>::iterator current_face;
    std::vector<bool> visibility;
    visibility.resize (mesh.tex_polygons[current_cam].size ());
    std::vector<UvIndex> indexes_uv_to_points;
    // for each current face

    //TODO change this
    pcl::PointXY nan_point;
    nan_point.x = std::numeric_limits<float>::quiet_NaN ();
    nan_point.y = std::numeric_limits<float>::quiet_NaN ();
    UvIndex u_null;
    u_null.idx_cloud = -1;
    u_null.idx_face = -1;

    int cpt_invisible=0;
    for (int idx_face = 0; idx_face <  static_cast<int> (mesh.tex_polygons[current_cam].size ()); ++idx_face)
    {
      //project each vertice, if one is out of view, stop
      pcl::PointXY uv_coord1;
      pcl::PointXY uv_coord2;
      pcl::PointXY uv_coord3;

      if (isFaceProjected (cameras[current_cam],
                           camera_cloud->points[mesh.tex_polygons[current_cam][idx_face].vertices[0]],
                           camera_cloud->points[mesh.tex_polygons[current_cam][idx_face].vertices[1]],
                           camera_cloud->points[mesh.tex_polygons[current_cam][idx_face].vertices[2]],
                           uv_coord1,
                           uv_coord2,
                           uv_coord3))
       {
        // face is in the camera's FOV

        // add UV coordinates
        projections->points.push_back (uv_coord1);
        projections->points.push_back (uv_coord2);
        projections->points.push_back (uv_coord3);

        // remember corresponding face
        UvIndex u1, u2, u3;
        u1.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[0];
        u2.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[1];
        u3.idx_cloud = mesh.tex_polygons[current_cam][idx_face].vertices[2];
        u1.idx_face = idx_face; u2.idx_face = idx_face; u3.idx_face = idx_face;
        indexes_uv_to_points.push_back (u1);
        indexes_uv_to_points.push_back (u2);
        indexes_uv_to_points.push_back (u3);

        //keep track of visibility
        visibility[idx_face] = true;
      }
      else
      {
        projections->points.push_back (nan_point);
        projections->points.push_back (nan_point);
        projections->points.push_back (nan_point);
        indexes_uv_to_points.push_back (u_null);
        indexes_uv_to_points.push_back (u_null);
        indexes_uv_to_points.push_back (u_null);
        //keep track of visibility
        visibility[idx_face] = false;
        cpt_invisible++;
      }
    }

    // projections contains all UV points of the current faces
    // indexes_uv_to_points links a uv point to its point in the camera cloud
    // visibility contains tells if a face was in the camera FOV (false = skip)

    // TODO handle case were no face could be projected
    if (visibility.size () - cpt_invisible !=0)
    {
        //create kdtree
        pcl::KdTreeFLANN<pcl::PointXY> kdtree;
        kdtree.setInputCloud (projections);

        std::vector<int> idxNeighbors;
        std::vector<float> neighborsSquaredDistance;
        // af first (idx_pcan < current_cam), check if some of the faces attached to previous cameras occlude the current faces
        // then (idx_pcam == current_cam), check for self occlusions. At this stage, we skip faces that were already marked as occluded
        cpt_invisible = 0;
        for (int idx_pcam = 0 ; idx_pcam <= current_cam ; ++idx_pcam)
        {
          // project all faces
          for (int idx_face = 0; idx_face <  static_cast<int> (mesh.tex_polygons[idx_pcam].size ()); ++idx_face)
          {

            if (idx_pcam == current_cam && !visibility[idx_face])
            {
              // we are now checking for self occlusions within the current faces
              // the current face was already declared as occluded.
              // therefore, it cannot occlude another face anymore => we skip it
              continue;
            }

            // project each vertice, if one is out of view, stop
            pcl::PointXY uv_coord1;
            pcl::PointXY uv_coord2;
            pcl::PointXY uv_coord3;

            if (isFaceProjected (cameras[current_cam],
                                 camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[0]],
                                 camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[1]],
                                 camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[2]],
                                 uv_coord1,
                                 uv_coord2,
                                 uv_coord3))
             {
              // face is in the camera's FOV
              //get its circumsribed circle
              double radius;
              pcl::PointXY center;
              // getTriangleCircumcenterAndSize (uv_coord1, uv_coord2, uv_coord3, center, radius);
              getTriangleCircumcscribedCircleCentroid(uv_coord1, uv_coord2, uv_coord3, center, radius); // this function yields faster results than getTriangleCircumcenterAndSize

              // get points inside circ.circle
              if (kdtree.radiusSearch (center, radius, idxNeighbors, neighborsSquaredDistance) > 0 )
              {
                // for each neighbor
                for (size_t i = 0; i < idxNeighbors.size (); ++i)
                {
                  if (std::max (camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[0]].z,
                                std::max (camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[1]].z, 
                                          camera_cloud->points[mesh.tex_polygons[idx_pcam][idx_face].vertices[2]].z))
                     < camera_cloud->points[indexes_uv_to_points[idxNeighbors[i]].idx_cloud].z)
                  {
                    // neighbor is farther than all the face's points. Check if it falls into the triangle
                    if (checkPointInsideTriangle(uv_coord1, uv_coord2, uv_coord3, projections->points[idxNeighbors[i]]))
                    {
                      // current neighbor is inside triangle and is closer => the corresponding face
                      visibility[indexes_uv_to_points[idxNeighbors[i]].idx_face] = false;
                      cpt_invisible++;
                      //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
                    }
                  }
                }
              }
             }
          }
        }
    }

    // now, visibility is true for each face that belongs to the current camera
    // if a face is not visible, we push it into the next one.

    if (static_cast<int> (mesh.tex_coordinates.size ()) <= current_cam)
    {
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
        std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > dummy_container;
#else
        std::vector<Eigen::Vector2f> dummy_container;
#endif
      mesh.tex_coordinates.push_back (dummy_container);
    }
    mesh.tex_coordinates[current_cam].resize (3 * visibility.size ());

    std::vector<pcl::Vertices> occluded_faces;
    occluded_faces.resize (visibility.size ());
    std::vector<pcl::Vertices> visible_faces;
    visible_faces.resize (visibility.size ());

    int cpt_occluded_faces = 0;
    int cpt_visible_faces = 0;

    for (size_t idx_face = 0 ; idx_face < visibility.size () ; ++idx_face)
    {
      if (visibility[idx_face])
      {
        // face is visible by the current camera copy UV coordinates
        mesh.tex_coordinates[current_cam][cpt_visible_faces * 3](0) = projections->points[idx_face*3].x;
        mesh.tex_coordinates[current_cam][cpt_visible_faces * 3](1) = projections->points[idx_face*3].y;

        mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](0) = projections->points[idx_face*3 + 1].x;
        mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 1](1) = projections->points[idx_face*3 + 1].y;

        mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](0) = projections->points[idx_face*3 + 2].x;
        mesh.tex_coordinates[current_cam][cpt_visible_faces * 3 + 2](1) = projections->points[idx_face*3 + 2].y;

        visible_faces[cpt_visible_faces] = mesh.tex_polygons[current_cam][idx_face];

        cpt_visible_faces++;
      }
      else
      {
        // face is occluded copy face into temp vector
        occluded_faces[cpt_occluded_faces] = mesh.tex_polygons[current_cam][idx_face];
        cpt_occluded_faces++;
      }
    }
    mesh.tex_coordinates[current_cam].resize (cpt_visible_faces*3);

    occluded_faces.resize (cpt_occluded_faces);
    mesh.tex_polygons.push_back (occluded_faces);

    visible_faces.resize (cpt_visible_faces);
    mesh.tex_polygons[current_cam].clear ();
    mesh.tex_polygons[current_cam] = visible_faces;

    int nb_faces = 0;
    for (int i = 0; i < static_cast<int> (mesh.tex_polygons.size ()); i++)
      nb_faces += static_cast<int> (mesh.tex_polygons[i].size ());
  }

  // we have been through all the cameras.
  // if any faces are left, they were not visible by any camera
  // we still need to produce uv coordinates for them

  if (mesh.tex_coordinates.size() <= cameras.size ())
  {
#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
        std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > dummy_container;
#else
        std::vector<Eigen::Vector2f> dummy_container;
#endif
   mesh.tex_coordinates.push_back(dummy_container);
   }


  for(size_t idx_face = 0 ; idx_face < mesh.tex_polygons[cameras.size()].size() ; ++idx_face)
  {
    Eigen::Vector2f UV1, UV2, UV3;
    UV1(0) = -1.0; UV1(1) = -1.0;
    UV2(0) = -1.0; UV2(1) = -1.0;
    UV3(0) = -1.0; UV3(1) = -1.0;
    mesh.tex_coordinates[cameras.size()].push_back(UV1);
    mesh.tex_coordinates[cameras.size()].push_back(UV2);
    mesh.tex_coordinates[cameras.size()].push_back(UV3);
  }

}

class FaceInfo
{
public:
        FaceInfo(float d,
                        float a,
                        float edge,
                        bool facingCam,
                        const pcl::PointXY & uv1,
                        const pcl::PointXY & uv2,
                        const pcl::PointXY & uv3,
                        const pcl::PointXY & center) :
                                distance(d),
                                angle(a),
                                longestEdgeSqrd(edge),
                                facingTheCam(facingCam),
                                uv_coord1(uv1),
                                uv_coord2(uv2),
                                uv_coord3(uv3),
                                uv_center(center)
        {}
        float distance;
        float angle;
        float longestEdgeSqrd;
        bool facingTheCam;
        pcl::PointXY uv_coord1;
        pcl::PointXY uv_coord2;
        pcl::PointXY uv_coord3;
        pcl::PointXY uv_center;
};

bool ptInTriangle(const pcl::PointXY & p0, const pcl::PointXY & p1, const pcl::PointXY & p2, const pcl::PointXY & p) {
    float A = 1/2 * (-p1.y * p2.x + p0.y * (-p1.x + p2.x) + p0.x * (p1.y - p2.y) + p1.x * p2.y);
    float sign = A < 0 ? -1 : 1;
    float s = (p0.y * p2.x - p0.x * p2.y + (p2.y - p0.y) * p.x + (p0.x - p2.x) * p.y) * sign;
    float t = (p0.x * p1.y - p0.y * p1.x + (p0.y - p1.y) * p.x + (p1.x - p0.x) * p.y) * sign;

    return s > 0 && t > 0 && (s + t) < 2 * A * sign;
}

template<typename PointInT> bool
pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
                pcl::TextureMesh &mesh,
                const pcl::texture_mapping::CameraVector &cameras,
                const rtabmap::ProgressState * state,
                std::vector<std::map<int, pcl::PointXY> > * vertexToPixels)
{

        if (mesh.tex_polygons.size () != 1)
                return false;

        typename pcl::PointCloud<PointInT>::Ptr mesh_cloud (new pcl::PointCloud<PointInT>);

        pcl::fromPCLPointCloud2 (mesh.cloud, *mesh_cloud);

        std::vector<pcl::Vertices> faces;
        faces.swap(mesh.tex_polygons[0]);

        mesh.tex_polygons.clear();
        mesh.tex_polygons.resize(cameras.size()+1);
        mesh.tex_coordinates.clear();
        mesh.tex_coordinates.resize(cameras.size()+1);

        // pre compute all cam inverse and visibility
        std::vector<std::map<int, FaceInfo > > visibleFaces(cameras.size());
        std::vector<Eigen::Affine3f> invCamTransform(cameras.size());
        std::vector<std::list<int> > faceCameras(faces.size());
        UINFO("Precompute visible faces per cam (%d faces, %d cams)", (int)faces.size(), (int)cameras.size());
        for (unsigned int current_cam = 0; current_cam < cameras.size(); ++current_cam)
        {
                UDEBUG("Texture camera %d...", current_cam);

                typename pcl::PointCloud<PointInT>::Ptr camera_cloud (new pcl::PointCloud<PointInT>);
                pcl::transformPointCloud(*mesh_cloud, *camera_cloud, cameras[current_cam].pose.inverse());

                std::vector<int> visibilityIndices;
                visibilityIndices.resize (faces.size ());
                pcl::PointCloud<pcl::PointXY>::Ptr projections (new pcl::PointCloud<pcl::PointXY>);
                projections->resize(faces.size()*3);
                std::map<float, int> sortedVisibleFaces;
                int oi=0;
                for(unsigned int idx_face=0; idx_face<faces.size(); ++idx_face)
                {
                        pcl::Vertices & face = faces[idx_face];

                        int j=oi*3;
                        pcl::PointXY & uv_coords1 = projections->at(j);
                        pcl::PointXY & uv_coords2 = projections->at(j+1);
                        pcl::PointXY & uv_coords3 = projections->at(j+2);
                        PointInT & pt0 = camera_cloud->points[face.vertices[0]];
                        PointInT & pt1 = camera_cloud->points[face.vertices[1]];
                        PointInT & pt2 = camera_cloud->points[face.vertices[2]];
                        if (isFaceProjected (cameras[current_cam],
                                        pt0,
                                        pt1,
                                        pt2,
                                        uv_coords1,
                                        uv_coords2,
                                        uv_coords3))
                        {
                                // check if the polygon is facing the camera, assuming counterclockwise normal
                                Eigen::Vector3f v0(
                                                uv_coords2.x - uv_coords1.x,
                                                uv_coords2.y - uv_coords1.y,
                                                0);
                                Eigen::Vector3f v1(
                                                uv_coords3.x - uv_coords1.x,
                                                uv_coords3.y - uv_coords1.y,
                                                0);
                                Eigen::Vector3f normal = v0.cross(v1);
                                float angle = normal.dot(Eigen::Vector3f(0.0f,0.0f,1.0f));
                                bool facingTheCam = angle>0.0f;
                                float distanceToCam = std::min(std::min(pt0.z, pt1.z), pt2.z);
                                float angleToCam = 0.0f;
                                Eigen::Vector3f e0 = Eigen::Vector3f(
                                                pt1.x - pt0.x,
                                                pt1.y - pt0.y,
                                                pt1.z - pt0.z);
                                Eigen::Vector3f e1 = Eigen::Vector3f(
                                                pt2.x - pt0.x,
                                                pt2.y - pt0.y,
                                                pt2.z - pt0.z);
                                Eigen::Vector3f e2 = Eigen::Vector3f(
                                                pt2.x - pt1.x,
                                                pt2.y - pt1.y,
                                                pt2.z - pt1.z);
                                if(facingTheCam && this->max_angle_)
                                {
                                        Eigen::Vector3f normal3D;
                                        normal3D = e0.cross(e1);
                                        angleToCam = pcl::getAngle3D(Eigen::Vector4f(normal3D[0], normal3D[1], normal3D[2], 0.0f), Eigen::Vector4f(0.0f,0.0f,-1.0f,0.0f));
                                }

                                // longest edge
                                float e0norm2 = e0[0]*e0[0] + e0[1]*e0[1] + e0[2]*e0[2];
                                float e1norm2 = e1[0]*e1[0] + e1[1]*e1[1] + e1[2]*e1[2];
                                float e2norm2 = e2[0]*e2[0] + e2[1]*e2[1] + e2[2]*e2[2];
                                float longestEdgeSqrd = std::max(std::max(e0norm2, e1norm2), e2norm2);

                                pcl::PointXY center;
                                center.x = (uv_coords1.x+uv_coords2.x+uv_coords3.x)/3.0f;
                                center.y = (uv_coords1.y+uv_coords2.y+uv_coords3.y)/3.0f;
                                visibleFaces[current_cam].insert(visibleFaces[current_cam].end(), std::make_pair(idx_face, FaceInfo(distanceToCam, angleToCam, longestEdgeSqrd, facingTheCam, uv_coords1, uv_coords2, uv_coords3, center)));
                                sortedVisibleFaces.insert(std::make_pair(distanceToCam, idx_face));
                                visibilityIndices[oi] = idx_face;
                                ++oi;
                        }
                }
                visibilityIndices.resize(oi);
                projections->resize(oi*3);
                UASSERT(projections->size() == visibilityIndices.size()*3);

                //filter occluded polygons
                //create kdtree
                pcl::KdTreeFLANN<pcl::PointXY> kdtree;
                kdtree.setInputCloud (projections);

                std::vector<int> idxNeighbors;
                std::vector<float> neighborsSquaredDistance;
                // af first (idx_pcan < current_cam), check if some of the faces attached to previous cameras occlude the current faces
                // then (idx_pcam == current_cam), check for self occlusions. At this stage, we skip faces that were already marked as occluded
                // project all faces
                std::set<int> occludedFaces;
                for (std::map<float, int>::iterator jter=sortedVisibleFaces.begin(); jter!=sortedVisibleFaces.end(); ++jter)
                //for (unsigned int idx = 0; idx<visibilityIndices.size(); ++idx)
                {
                        int idx_face = jter->second;
                        //int idx_face = visibilityIndices[idx];
                        std::map<int, FaceInfo>::iterator iter= visibleFaces[current_cam].find(idx_face);
                        UASSERT(iter != visibleFaces[current_cam].end());

                        FaceInfo & info = iter->second;

                        // face is in the camera's FOV
                        //get its circumsribed circle
                        double radius;
                        pcl::PointXY center;
                        // getTriangleCircumcenterAndSize (info.uv_coord1, info.uv_coord2, info.uv_coord3, center, radius);
                        getTriangleCircumcscribedCircleCentroid(info.uv_coord1, info.uv_coord2, info.uv_coord3, center, radius); // this function yields faster results than getTriangleCircumcenterAndSize

                        // get points inside circ.circle
                        if (kdtree.radiusSearch (center, radius, idxNeighbors, neighborsSquaredDistance) > 0 )
                        {
                                // for each neighbor
                                for (size_t i = 0; i < idxNeighbors.size (); ++i)
                                {
                                        int neighborFaceIndex = idxNeighbors[i]/3;
                                        //std::map<int, FaceInfo>::iterator jter= visibleFaces[current_cam].find(visibilityIndices[neighborFaceIndex]);
                                        //if(jter != visibleFaces[current_cam].end())
                                        {
                                                if (std::max(camera_cloud->points[faces[idx_face].vertices[0]].z,
                                                                        std::max (camera_cloud->points[faces[idx_face].vertices[1]].z,
                                                                                        camera_cloud->points[faces[idx_face].vertices[2]].z))
                                                        < camera_cloud->points[faces[visibilityIndices[neighborFaceIndex]].vertices[idxNeighbors[i]%3]].z)
                                                //if (info.distance < jter->second.distance)
                                                {
                                                        // neighbor is farther than all the face's points. Check if it falls into the triangle
                                                        if (checkPointInsideTriangle(info.uv_coord1, info.uv_coord2, info.uv_coord3, projections->at(idxNeighbors[i])))
                                                        {
                                                                // current neighbor is inside triangle and is closer => the corresponding face
                                                                occludedFaces.insert(visibilityIndices[neighborFaceIndex]);
                                                                //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
                                                        }
                                                }
                                        }
                                }
                        }
                }

                // remove occluded faces
                for(std::set<int>::iterator iter= occludedFaces.begin(); iter!=occludedFaces.end(); ++iter)
                {
                        visibleFaces[current_cam].erase(*iter);
                }

                // filter clusters
                int clusterFaces = 0;

                std::vector<pcl::Vertices> polygons(visibleFaces[current_cam].size());
                std::vector<int> polygon_to_face_index(visibleFaces[current_cam].size());
                oi =0;
                for(std::map<int, FaceInfo>::iterator iter=visibleFaces[current_cam].begin(); iter!=visibleFaces[current_cam].end(); ++iter)
                {
                        polygons[oi].vertices.resize(3);
                        polygons[oi].vertices[0] = faces[iter->first].vertices[0];
                        polygons[oi].vertices[1] = faces[iter->first].vertices[1];
                        polygons[oi].vertices[2] = faces[iter->first].vertices[2];
                        polygon_to_face_index[oi] = iter->first;
                        ++oi;
                }

                std::vector<std::set<int> > neighbors;
                std::vector<std::set<int> > vertexToPolygons;
                rtabmap::util3d::createPolygonIndexes(polygons,
                                (int)camera_cloud->size(),
                                neighbors,
                                vertexToPolygons);
                std::list<std::list<int> > clusters = rtabmap::util3d::clusterPolygons(
                                neighbors,
                                min_cluster_size_);
                std::set<int> polygonsKept;
                for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
                {
                        for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
                        {
                                polygonsKept.insert(polygon_to_face_index[*jter]);
                                faceCameras[polygon_to_face_index[*jter]].push_back(current_cam);
                        }
                }

                for(std::map<int, FaceInfo>::iterator iter=visibleFaces[current_cam].begin(); iter!=visibleFaces[current_cam].end();)
                {
                        if(polygonsKept.find(iter->first) == polygonsKept.end())
                        {
                                visibleFaces[current_cam].erase(iter++);
                                ++clusterFaces;
                        }
                        else
                        {
                                ++iter;
                        }
                }

                std::string msg = uFormat("Processed camera %d/%d: %d occluded and %d spurious polygons out of %d", (int)current_cam+1, (int)cameras.size(), (int)occludedFaces.size(), clusterFaces, (int)visibilityIndices.size());
                UINFO(msg.c_str());
                if(state && !state->callback(msg))
                {
                        //cancelled!
                        UWARN("Texturing cancelled!");
                        return false;
                }
        }

        std::string msg = uFormat("Texturing %d polygons...", (int)faces.size());
        UINFO(msg.c_str());
        if(state && !state->callback(msg))
        {
                //cancelled!
                UWARN("Texturing cancelled!");
                return false;
        }
        int textured = 0;
        if(vertexToPixels)
        {
                *vertexToPixels = std::vector<std::map<int, pcl::PointXY> >(mesh_cloud->size());
        }
        for(unsigned int idx_face=0; idx_face<faces.size(); ++idx_face)
        {
                if((idx_face+1)%10000 == 0)
                {
                        UDEBUG("face %d/%d", idx_face+1, (int)faces.size());
                        if(state && !state->callback(uFormat("Textured %d/%d of %d polygons", textured, idx_face+1, (int)faces.size())))
                        {
                                //cancelled!
                                UWARN("Texturing cancelled!");
                                return false;
                        }
                }
                pcl::Vertices & face = faces[idx_face];

                int cameraIndex = -1;
                float smallestWeight = std::numeric_limits<float>::max();
                bool depthSet = false;
                pcl::PointXY uv_coords[3];
                for (std::list<int>::iterator camIter = faceCameras[idx_face].begin(); camIter!=faceCameras[idx_face].end(); ++camIter)
                {
                        int current_cam = *camIter;
                        std::map<int, FaceInfo>::iterator iter = visibleFaces[current_cam].find(idx_face);
                        UASSERT(iter != visibleFaces[current_cam].end());
                        if (iter->second.facingTheCam && (max_angle_ <=0.0f || iter->second.angle <= max_angle_))
                        {
                                float distanceToCam = iter->second.distance;
                                float vx = (iter->second.uv_coord1.x+iter->second.uv_coord2.x+ iter->second.uv_coord3.x)/3.0f-0.5f;
                                float vy = (iter->second.uv_coord1.y+iter->second.uv_coord2.y+ iter->second.uv_coord3.y)/3.0f-0.5f;
                                float distanceToCenter = vx*vx+vy*vy;

                                cv::Mat depth = cameras[current_cam].depth;
                                bool currentDepthSet = false;
                                float maxDepthError = max_depth_error_==0.0f?std::sqrt(iter->second.longestEdgeSqrd)*2.0f : max_depth_error_;
                                if(!cameras[current_cam].depth.empty() && maxDepthError > 0.0f)
                                {
                                        float d1 = depth.type() == CV_32FC1?
                                                        depth.at<float>((1.0f-iter->second.uv_coord1.y)*depth.rows, iter->second.uv_coord1.x*depth.cols):
                                                        float(depth.at<unsigned short>((1.0f-iter->second.uv_coord1.y)*depth.rows, iter->second.uv_coord1.x*depth.cols))/1000.0f;
                                        float d2 = depth.type() == CV_32FC1?
                                                        depth.at<float>((1.0f-iter->second.uv_coord2.y)*depth.rows, iter->second.uv_coord2.x*depth.cols):
                                                        float(depth.at<unsigned short>((1.0f-iter->second.uv_coord2.y)*depth.rows, iter->second.uv_coord2.x*depth.cols))/1000.0f;
                                        float d3 = depth.type() == CV_32FC1?
                                                        depth.at<float>((1.0f-iter->second.uv_coord3.y)*depth.rows, iter->second.uv_coord3.x*depth.cols):
                                                        float(depth.at<unsigned short>((1.0f-iter->second.uv_coord3.y)*depth.rows, iter->second.uv_coord3.x*depth.cols))/1000.0f;
                                        if(d1 <= 0.0f || !std::isfinite(d1) || d2 <= 0.0f || !std::isfinite(d2) || d3 <= 0.0f || !std::isfinite(d3))
                                        {
                                                if(depthSet)
                                                {
                                                        // ignore pixels with no depth
                                                        continue;
                                                }
                                                else if(d1 > 0.0f && std::isfinite(d1) && fabs(d1 - distanceToCam) > maxDepthError)
                                                {
                                                        // ignore pixels with too much depth error
                                                        continue;
                                                }
                                                else if(d2 > 0.0f && std::isfinite(d2) && fabs(d2 - distanceToCam) > maxDepthError)
                                                {
                                                        // ignore pixels with too much depth error
                                                        continue;
                                                }
                                                else if(d3 > 0.0f && std::isfinite(d3) && fabs(d3 - distanceToCam) > maxDepthError)
                                                {
                                                        // ignore pixels with too much depth error
                                                        continue;
                                                }
                                                //else it could be a window for which no depth is available on any cameras
                                        }
                                        else
                                        {
                                                if(fabs(d1 - distanceToCam) > maxDepthError ||
                                                        fabs(d2 - distanceToCam) > maxDepthError ||
                                                        fabs(d3 - distanceToCam) > maxDepthError)
                                                {
                                                        // ignore pixels with too much depth error
                                                        continue;
                                                }
                                                currentDepthSet = true;
                                        }
                                }

                                if(vertexToPixels)
                                {
                                        vertexToPixels->at(face.vertices[0]).insert(std::make_pair(current_cam, iter->second.uv_coord1));
                                        vertexToPixels->at(face.vertices[1]).insert(std::make_pair(current_cam, iter->second.uv_coord2));
                                        vertexToPixels->at(face.vertices[2]).insert(std::make_pair(current_cam, iter->second.uv_coord3));
                                }

                                //UDEBUG("Process polygon %d cam =%d distanceToCam=%f", idx_face, current_cam, distanceToCam);

                                if(distanceToCenter <= smallestWeight || (!depthSet && currentDepthSet))
                                {
                                        cameraIndex = current_cam;
                                        smallestWeight = distanceToCenter;
                                        uv_coords[0] = iter->second.uv_coord1;
                                        uv_coords[1] = iter->second.uv_coord2;
                                        uv_coords[2] = iter->second.uv_coord3;
                                        if(!depthSet && currentDepthSet)
                                        {
                                                depthSet = true;
                                        }
                                }
                        }
                }

                if(cameraIndex >= 0)
                {
                        ++textured;
                        mesh.tex_polygons[cameraIndex].push_back(face);
                        mesh.tex_coordinates[cameraIndex].push_back(Eigen::Vector2f(uv_coords[0].x, uv_coords[0].y));
                        mesh.tex_coordinates[cameraIndex].push_back(Eigen::Vector2f(uv_coords[1].x, uv_coords[1].y));
                        mesh.tex_coordinates[cameraIndex].push_back(Eigen::Vector2f(uv_coords[2].x, uv_coords[2].y));
                }
                else
                {
                        mesh.tex_polygons[cameras.size()].push_back(face);
                        mesh.tex_coordinates[cameras.size()].push_back(Eigen::Vector2f(-1.0,-1.0));
                        mesh.tex_coordinates[cameras.size()].push_back(Eigen::Vector2f(-1.0,-1.0));
                        mesh.tex_coordinates[cameras.size()].push_back(Eigen::Vector2f(-1.0,-1.0));
                }
        }
        UINFO("Process %d polygons...done! (%d textured)", (int)faces.size(), textured);

        return true;
}

template<typename PointInT> inline void
pcl::TextureMapping<PointInT>::getTriangleCircumcenterAndSize(const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circomcenter, double &radius)
{
  // we simplify the problem by translating the triangle's origin to its first point
  pcl::PointXY ptB, ptC;
  ptB.x = p2.x - p1.x; ptB.y = p2.y - p1.y; // B'=B-A
  ptC.x = p3.x - p1.x; ptC.y = p3.y - p1.y; // C'=C-A

  double D = 2.0*(ptB.x*ptC.y - ptB.y*ptC.x); // D'=2(B'x*C'y - B'y*C'x)

  // Safety check to avoid division by zero
  if(D == 0)
  {
    circomcenter.x = p1.x;
    circomcenter.y = p1.y;
  }
  else
  {
    // compute squares once
    double bx2 = ptB.x * ptB.x; // B'x^2
    double by2 = ptB.y * ptB.y; // B'y^2
    double cx2 = ptC.x * ptC.x; // C'x^2
    double cy2 = ptC.y * ptC.y; // C'y^2

    // compute circomcenter's coordinates (translate back to original coordinates)
    circomcenter.x = static_cast<float> (p1.x + (ptC.y*(bx2 + by2) - ptB.y*(cx2 + cy2)) / D);
    circomcenter.y = static_cast<float> (p1.y + (ptB.x*(cx2 + cy2) - ptC.x*(bx2 + by2)) / D);
  }

  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));
}

template<typename PointInT> inline void
pcl::TextureMapping<PointInT>::getTriangleCircumcscribedCircleCentroid ( const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, pcl::PointXY &circumcenter, double &radius)
{
  // compute centroid's coordinates (translate back to original coordinates)
  circumcenter.x = static_cast<float> (p1.x + p2.x + p3.x ) / 3;
  circumcenter.y = static_cast<float> (p1.y + p2.y + p3.y ) / 3;
  double r1 = (circumcenter.x - p1.x) * (circumcenter.x - p1.x) + (circumcenter.y - p1.y) * (circumcenter.y - p1.y)  ;
  double r2 = (circumcenter.x - p2.x) * (circumcenter.x - p2.x) + (circumcenter.y - p2.y) * (circumcenter.y - p2.y)  ;
  double r3 = (circumcenter.x - p3.x) * (circumcenter.x - p3.x) + (circumcenter.y - p3.y) * (circumcenter.y - p3.y)  ;

  // radius
  radius = std::sqrt( std::max( r1, std::max( r2, r3) )) ;
}


template<typename PointInT> inline bool
pcl::TextureMapping<PointInT>::getPointUVCoordinates(const PointInT &pt, const Camera &cam, pcl::PointXY &UV_coordinates)
{
        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 camera's image plane
                UV_coordinates.x = static_cast<float> ((focal_x * (pt.x / pt.z) + cx) / sizeX); //horizontal
                UV_coordinates.y = static_cast<float> ((focal_y * (pt.y / pt.z) + cy) / sizeY); //vertical

                if(cam.roi.size() == 4)
                {
                        if( UV_coordinates.x < cam.roi[0]/sizeX ||
                                UV_coordinates.y < cam.roi[1]/sizeY ||
                                UV_coordinates.x > (cam.roi[0]+cam.roi[2])/sizeX ||
                                UV_coordinates.y > (cam.roi[1]+cam.roi[3])/sizeY)
                        {
                                // point is NOT in region of interest of the camera
                                UV_coordinates.x = -1.0f;
                                UV_coordinates.y = -1.0f;
                                return false;
                        }
                }

                // point is visible!
                if (UV_coordinates.x >= 0.0 && UV_coordinates.x <= 1.0 && UV_coordinates.y >= 0.0 && UV_coordinates.y <= 1.0)
                {
                        // point is visible by the camera
                        // original code of PCL inverted y
                        UV_coordinates.y = 1.0f - UV_coordinates.y;
                        return (true);
                }
        }

        // point is NOT visible by the camera
        UV_coordinates.x = -1.0f;
        UV_coordinates.y = -1.0f;
        return (false); // point was not visible by the camera
}

template<typename PointInT> inline bool
pcl::TextureMapping<PointInT>::checkPointInsideTriangle(const pcl::PointXY &p1, const pcl::PointXY &p2, const pcl::PointXY &p3, const pcl::PointXY &pt)
{
   // Compute vectors
   Eigen::Vector2d v0, v1, v2;
   v0(0) = p3.x - p1.x; v0(1) = p3.y - p1.y; // v0= C - A
   v1(0) = p2.x - p1.x; v1(1) = p2.y - p1.y; // v1= B - A
   v2(0) = pt.x - p1.x; v2(1) = pt.y - p1.y; // v2= P - A

   // Compute dot products
   double dot00 = v0.dot(v0); // dot00 = dot(v0, v0)
   double dot01 = v0.dot(v1); // dot01 = dot(v0, v1)
   double dot02 = v0.dot(v2); // dot02 = dot(v0, v2)
   double dot11 = v1.dot(v1); // dot11 = dot(v1, v1)
   double dot12 = v1.dot(v2); // dot12 = dot(v1, v2)

   // Compute barycentric coordinates
   double invDenom = 1.0 / (dot00*dot11 - dot01*dot01);
   double u = (dot11*dot02 - dot01*dot12) * invDenom;
   double v = (dot00*dot12 - dot01*dot02) * invDenom;

   // Check if point is in triangle
   return ((u >= 0) && (v >= 0) && (u + v < 1));
}

template<typename PointInT> inline bool
pcl::TextureMapping<PointInT>::isFaceProjected (const Camera &camera, const PointInT &p1, const PointInT &p2, const PointInT &p3, pcl::PointXY &proj1, pcl::PointXY &proj2, pcl::PointXY &proj3)
{
        return getPointUVCoordinates(p1, camera, proj1)
                        &&
                        getPointUVCoordinates(p2, camera, proj2)
                        &&
                        getPointUVCoordinates(p3, camera, proj3);
}

#define PCL_INSTANTIATE_TextureMapping(T)                \
    template class PCL_EXPORTS pcl::TextureMapping<T>;

#endif /* TEXTURE_MAPPING_HPP_ */