TextureMapping Speedup OpenMP

texturemapping_multicamera_speedup.cpp

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


        // 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
            #pragma omp parallel for shared(visibility,cpt_invisible)
          for (int idx_face = 0; idx_face <  static_cast<int> (mesh.tex_polygons[idx_pcam].size ()); ++idx_face)
          {
              std::vector<int> idxNeighbors;
              std::vector<float> neighborsSquaredDistance;

            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
                        #pragma omp critical(dataupdate)
                        {
                      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
                    }
                  }
                }
              }
             }
          }
        }
    }