<commit_before>// This file is part of OpenMVG, an Open Multiple View Geometry C++ library.

// Copyright (c) 2015 <NAME>.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "openMVG/sfm/sfm_data_filters_frustum.hpp"

#include "openMVG/cameras/Camera_Pinhole.hpp"
#include "openMVG/geometry/pose3.hpp"
#include "openMVG/image/pixel_types.hpp"
#include "openMVG/sfm/sfm_data.hpp"
#include "openMVG/stl/stl.hpp"

#include "third_party/progress/progress_display.hpp"

#include <fstream>
#include <iomanip>
#include <iterator>

namespace openMVG {
namespace sfm {

using namespace openMVG::cameras;
using namespace openMVG::geometry;
using namespace openMVG::geometry::halfPlane;

std::string RgbPLYString(const image::RGBColor& color);

// Constructor
Frustum_Filter::Frustum_Filter
(
  const SfM_Data & sfm_data,
  const double zNear,
  const double zFar,
  const NearFarPlanesT & z_near_z_far
)
: z_near_z_far_perView(z_near_z_far)
{
  //-- Init Z_Near & Z_Far for all valid views
  init_z_near_z_far_depth(sfm_data, zNear, zFar);
  const bool bComputed_Z = (zNear == -1. && zFar == -1.) && !sfm_data.structure.empty();
  _bTruncated = (zNear != -1. && zFar != -1.) || bComputed_Z;
  initFrustum(sfm_data);
}

// Init a frustum for each valid views of the SfM scene
void Frustum_Filter::initFrustum
(
  const SfM_Data & sfm_data
)
{
  for (NearFarPlanesT::const_iterator it = z_near_z_far_perView.begin();
      it != z_near_z_far_perView.end(); ++it)
  {
    const View * view = sfm_data.GetViews().at(it->first).get();
    if (!sfm_data.IsPoseAndIntrinsicDefined(view))
      continue;
    Intrinsics::const_iterator iterIntrinsic = sfm_data.GetIntrinsics().find(view->id_intrinsic);
    if (!isPinhole(iterIntrinsic->second->getType()))
      continue;

    const Pose3 pose = sfm_data.GetPoseOrDie(view);

    const Pinhole_Intrinsic * cam = dynamic_cast<const Pinhole_Intrinsic*>(iterIntrinsic->second.get());
    if (!cam)
      continue;

    if (!_bTruncated) // use infinite frustum
    {
      const Frustum f(
        cam->w(), cam->h(), cam->K(),
        pose.rotation(), pose.center());
      frustum_perView[view->id_view] = f;
    }
    else // use truncated frustum with defined Near and Far planes
    {
      const Frustum f(cam->w(), cam->h(), cam->K(),
        pose.rotation(), pose.center(), it->second.first, it->second.second);
      frustum_perView[view->id_view] = f;
    }
  }
}

Pair_Set Frustum_Filter::getFrustumIntersectionPairs
(
  const std::vector<HalfPlaneObject>& bounding_volume
)
const
{
  Pair_Set pairs;
  // List active view Id
  std::vector<IndexT> viewIds;
  viewIds.reserve(z_near_z_far_perView.size());
  std::transform(z_near_z_far_perView.cbegin(), z_near_z_far_perView.cend(),
    std::back_inserter(viewIds), stl::RetrieveKey());

  C_Progress_display my_progress_bar(
    viewIds.size() * (viewIds.size()-1)/2,
    std::cout, "\nCompute frustum intersection\n");

  // Exhaustive comparison (use the fact that the intersect function is symmetric)
#ifdef OPENMVG_USE_OPENMP
  #pragma omp parallel for
#endif
  for (int i = 0; i < (int)viewIds.size(); ++i)
  {
    // Prepare vector of intersecting objects (within loop to keep it
    // thread-safe)
    std::vector<HalfPlaneObject> objects = bounding_volume;
    objects.insert(objects.end(),
                   { frustum_perView.at(viewIds[i]), HalfPlaneObject() });

    for (size_t j = i+1; j < viewIds.size(); ++j)
    {
      objects.back() = frustum_perView.at(viewIds[j]);
      if (intersect(objects))
      {
#ifdef OPENMVG_USE_OPENMP
        #pragma omp critical
#endif
        {
          pairs.insert({viewIds[i], viewIds[j]});
        }
      }
      // Progress bar update
      ++my_progress_bar;
    }
  }
  return pairs;
}

// Export defined frustum in PLY file for viewing
bool Frustum_Filter::export_Ply
(
  const std::string & filename,
  bool colorize
)
const
{
  std::ofstream of(filename.c_str());
  if (!of.is_open())
    return false;
  // Vertex count evaluation
  // Faces count evaluation
  size_t vertex_count = 0;
  size_t face_count = 0;
  for (FrustumsT::const_iterator it = frustum_perView.begin();
    it != frustum_perView.end(); ++it)
  {
    if (it->second.isInfinite())
    {
      vertex_count += 5;
      face_count += 5; // 4 triangles + 1 quad
    }
    else // truncated
    {
      vertex_count += 8;
      face_count += 6; // 6 quads
    }
  }

  of << std::fixed << std::setprecision (std::numeric_limits<double>::digits10 + 1);

  of << "ply" << '\n'
    << "format ascii 1.0" << '\n'
    << "element vertex " << vertex_count << '\n'
    << "property double x" << '\n'
    << "property double y" << '\n'
    << "property double z" << '\n'
    << "element face " << face_count << '\n'
    << "property list uchar int vertex_index" << '\n';

  if (colorize) {
    of << "property uchar red" << "\n"
    << "property uchar green" << "\n"
    << "property uchar blue" << "\n";
  }

  of << "end_header" << '\n';

  // Export frustums points
  for (FrustumsT::const_iterator it = frustum_perView.begin();
    it != frustum_perView.end(); ++it)
  {
    const std::vector<Vec3> & points = it->second.frustum_points();
    for (size_t i=0; i < points.size(); ++i)
      of << points[i].transpose() << '\n';
  }

  // Export frustums faces
  IndexT count = 0;
  std::string color_top_face = colorize ? RgbPLYString(image::RED) : "";
  std::string color_other_face = colorize ? RgbPLYString(image::GRAY) : "";
  for (FrustumsT::const_iterator it = frustum_perView.begin();
    it != frustum_perView.end(); ++it)
  {
    if (it->second.isInfinite()) // infinite frustum: drawn normalized cone: 4 faces
    {
      of << "3 " << count + 0 << ' ' << count + 1 << ' ' << count + 2 << color_other_face << '\n'
        << "3 " << count + 0 << ' ' << count + 2 << ' ' << count + 3 << color_other_face << '\n'
        << "3 " << count + 0 << ' ' << count + 3 << ' ' << count + 4 << color_top_face << '\n'
        << "3 " << count + 0 << ' ' << count + 4 << ' ' << count + 1 << color_other_face << '\n'
        << "4 " << count + 1 << ' ' << count + 2 << ' ' << count + 3 << ' ' << count + 4 << color_other_face << '\n';
      count += 5;
    }
    else // truncated frustum: 6 faces
    {
      of << "4 " << count + 0 << ' ' << count + 1 << ' ' << count + 2 << ' ' << count + 3 << color_other_face << '\n'
        << "4 " << count + 0 << ' ' << count + 1 << ' ' << count + 5 << ' ' << count + 4 << color_other_face << '\n'
        << "4 " << count + 1 << ' ' << count + 5 << ' ' << count + 6 << ' ' << count + 2 << color_top_face << '\n'
        << "4 " << count + 3 << ' ' << count + 7 << ' ' << count + 6 << ' ' << count + 2 << color_other_face << '\n'
        << "4 " << count + 0 << ' ' << count + 4 << ' ' << count + 7 << ' ' << count + 3 << color_other_face << '\n'
        << "4 " << count + 4 << ' ' << count + 5 << ' ' << count + 6 << ' ' << count + 7 << color_other_face << '\n';
      count += 8;
    }
  }
  of.flush();
  const bool bOk = of.good();
  of.close();
  return bOk;
}

void Frustum_Filter::init_z_near_z_far_depth
(
  const SfM_Data & sfm_data,
  const double zNear,
  const double zFar
)<commit_msg>(FrustumFilter) Fix top face not being colored correctly (#1773)<commit_after>// This file is part of OpenMVG, an Open Multiple View Geometry C++ library.

// Copyright (c) 2015 <NAME>.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "openMVG/sfm/sfm_data_filters_frustum.hpp"

#include "openMVG/cameras/Camera_Pinhole.hpp"
#include "openMVG/geometry/pose3.hpp"
#include "openMVG/image/pixel_types.hpp"
#include "openMVG/sfm/sfm_data.hpp"
#include "openMVG/stl/stl.hpp"

#include "third_party/progress/progress_display.hpp"

#include <fstream>
#include <iomanip>
#include <iterator>

namespace openMVG {
namespace sfm {

using namespace openMVG::cameras;
using namespace openMVG::geometry;
using namespace openMVG::geometry::halfPlane;

std::string RgbPLYString(const image::RGBColor& color);

// Constructor
Frustum_Filter::Frustum_Filter
(
  const SfM_Data & sfm_data,
  const double zNear,
  const double zFar,
  const NearFarPlanesT & z_near_z_far
)
: z_near_z_far_perView(z_near_z_far)
{
  //-- Init Z_Near & Z_Far for all valid views
  init_z_near_z_far_depth(sfm_data, zNear, zFar);
  const bool bComputed_Z = (zNear == -1. && zFar == -1.) && !sfm_data.structure.empty();
  _bTruncated = (zNear != -1. && zFar != -1.) || bComputed_Z;
  initFrustum(sfm_data);
}

// Init a frustum for each valid views of the SfM scene
void Frustum_Filter::initFrustum
(
  const SfM_Data & sfm_data
)
{
  for (NearFarPlanesT::const_iterator it = z_near_z_far_perView.begin();
      it != z_near_z_far_perView.end(); ++it)
  {
    const View * view = sfm_data.GetViews().at(it->first).get();
    if (!sfm_data.IsPoseAndIntrinsicDefined(view))
      continue;
    Intrinsics::const_iterator iterIntrinsic = sfm_data.GetIntrinsics().find(view->id_intrinsic);
    if (!isPinhole(iterIntrinsic->second->getType()))
      continue;

    const Pose3 pose = sfm_data.GetPoseOrDie(view);

    const Pinhole_Intrinsic * cam = dynamic_cast<const Pinhole_Intrinsic*>(iterIntrinsic->second.get());
    if (!cam)
      continue;

    if (!_bTruncated) // use infinite frustum
    {
      const Frustum f(
        cam->w(), cam->h(), cam->K(),
        pose.rotation(), pose.center());
      frustum_perView[view->id_view] = f;
    }
    else // use truncated frustum with defined Near and Far planes
    {
      const Frustum f(cam->w(), cam->h(), cam->K(),
        pose.rotation(), pose.center(), it->second.first, it->second.second);
      frustum_perView[view->id_view] = f;
    }
  }
}

Pair_Set Frustum_Filter::getFrustumIntersectionPairs
(
  const std::vector<HalfPlaneObject>& bounding_volume
)
const
{
  Pair_Set pairs;
  // List active view Id
  std::vector<IndexT> viewIds;
  viewIds.reserve(z_near_z_far_perView.size());
  std::transform(z_near_z_far_perView.cbegin(), z_near_z_far_perView.cend(),
    std::back_inserter(viewIds), stl::RetrieveKey());

  C_Progress_display my_progress_bar(
    viewIds.size() * (viewIds.size()-1)/2,
    std::cout, "\nCompute frustum intersection\n");

  // Exhaustive comparison (use the fact that the intersect function is symmetric)
#ifdef OPENMVG_USE_OPENMP
  #pragma omp parallel for
#endif
  for (int i = 0; i < (int)viewIds.size(); ++i)
  {
    // Prepare vector of intersecting objects (within loop to keep it
    // thread-safe)
    std::vector<HalfPlaneObject> objects = bounding_volume;
    objects.insert(objects.end(),
                   { frustum_perView.at(viewIds[i]), HalfPlaneObject() });

    for (size_t j = i+1; j < viewIds.size(); ++j)
    {
      objects.back() = frustum_perView.at(viewIds[j]);
      if (intersect(objects))
      {
#ifdef OPENMVG_USE_OPENMP
        #pragma omp critical
#endif
        {
          pairs.insert({viewIds[i], viewIds[j]});
        }
      }
      // Progress bar update
      ++my_progress_bar;
    }
  }
  return pairs;
}

// Export defined frustum in PLY file for viewing
bool Frustum_Filter::export_Ply
(
  const std::string & filename,
  bool colorize
)
const
{
  std::ofstream of(filename.c_str());
  if (!of.is_open())
    return false;
  // Vertex count evaluation
  // Faces count evaluation
  size_t vertex_count = 0;
  size_t face_count = 0;
  for (FrustumsT::const_iterator it = frustum_perView.begin();
    it != frustum_perView.end(); ++it)
  {
    if (it->second.isInfinite())
    {
      vertex_count += 5;
      face_count += 5; // 4 triangles + 1 quad
    }
    else // truncated
    {
      vertex_count += 8;
      face_count += 6; // 6 quads
    }
  }

  of << std::fixed << std::setprecision (std::numeric_limits<double>::digits10 + 1);

  of << "ply" << '\n'
    << "format ascii 1.0" << '\n'
    << "element vertex " << vertex_count << '\n'
    << "property double x" << '\n'
    << "property double y" << '\n'
    << "property double z" << '\n'
    << "element face " << face_count << '\n'
    << "property list uchar int vertex_index" << '\n';

  if (colorize) {
    of << "property uchar red" << "\n"
    << "property uchar green" << "\n"
    << "property uchar blue" << "\n";
  }

  of << "end_header" << '\n';

  // Export frustums points
  for (FrustumsT::const_iterator it = frustum_perView.begin();
    it != frustum_perView.end(); ++it)
  {
    const std::vector<Vec3> & points = it->second.frustum_points();
    for (size_t i=0; i < points.size(); ++i)
      of << points[i].transpose() << '\n';
  }

  // Export frustums faces
  IndexT count = 0;
  std::string color_top_face = colorize ? RgbPLYString(image::RED) : "";
  std::string color_other_face = colorize ? RgbPLYString(image::GRAY) : "";
  for (FrustumsT::const_iterator it = frustum_perView.begin();
    it != frustum_perView.end(); ++it)
  {
    if (it->second.isInfinite()) // infinite frustum: drawn normalized cone: 4 faces
    {
      of << "3 " << count + 0 << ' ' << count + 1 << ' ' << count + 2 << color_top_face << '\n'
        << "3 " << count + 0 << ' ' << count + 2 << ' ' << count + 3 << color_other_face << '\n'
        << "3 " << count + 0 << ' ' << count + 3 << ' ' << count + 4 << color_other_face << '\n'
        << "3 " << count + 0 << ' ' << count + 4 << ' ' << count + 1 << color_other_face << '\n'
        << "4 " << count + 1 << ' ' << count + 2 << ' ' << count + 3 << ' ' << count + 4 << color_other_face << '\n';
      count += 5;
    }
    else // truncated frustum: 6 faces
    {
      of << "4 " << count + 0 << ' ' << count + 1 << ' ' << count + 2 << ' ' << count + 3 << color_other_face << '\n'
        << "4 " << count + 0 << ' ' << count + 1 << ' ' << count + 5 << ' ' << count + 4 << color_top_face << '\n'
        << "4 " << count + 1 << ' ' << count + 5 << ' ' << count + 6 << ' ' << count + 2 << color_other_face << '\n'
        << "4 " << count + 3 << ' ' << count + 7 << ' ' << count + 6 << ' ' << count + 2 << color_other_face << '\n'
        << "4 " << count + 0 << ' ' << count + 4 << ' ' << count + 7 << ' ' << count + 3 << color_other_face << '\n'
        << "4 " << count + 4 << ' ' << count + 5 << ' ' << count + 6 << ' ' << count + 7 << color_other_face << '\n';
      count += 8;
    }
  }
  of.flush();
  const bool bOk = of.good();
  of.close();
  return bOk;
}

void Frustum_Filter::init_z_near_z_far_depth
(
  const SfM_Data & sfm_data,
  const double zNear,
  const double zFar
)
{'fraction_non_alphanumeric': 0.17133319708427006, 'fraction_numerical': 0.01253491382246747, 'mean_word_length': 3.0098333788582354, 'pattern_counts': {'":': 0, '<': 557, '<?xml version=': 0, '>': 59, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2225679', 'n_tokens_mistral': 5543, 'n_tokens_neox': 5253, 'n_words': 1244}