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

// Copyright (c) 2017 <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/cameras/Camera_Pinhole.hpp"
#include "openMVG/geometry/pose3.hpp"
#include "openMVG/geometry/frustum.hpp"
#include "openMVG/image/image_io.hpp"
#include "openMVG/sfm/sfm_data.hpp"
#include "openMVG/sfm/sfm_data_io.hpp"
#include "openMVG/stl/stl.hpp"

#include "config.h"

#include "third_party/cmdLine/cmdLine.h"
#include "third_party/progress/progress_display.hpp"
#include "third_party/stlplus3/filesystemSimplified/file_system.hpp"

#include <iostream>
#include <fstream>

using namespace openMVG;
using namespace openMVG::sfm;
using namespace openMVG::geometry;
using namespace openMVG::image;
using namespace openMVG::cameras;

/**
* @brief Compute normal using the mean of the direction for each points
* @param sfm_data Input scene data
* @param vec_nor A vector of normal for each point (in same order as ColorizeTrack output)
*/
void ComputeNormals( const SfM_Data & sfm_data , std::vector<Vec3> & vec_nor )
{
  vec_nor.resize( sfm_data.GetLandmarks().size() );

  IndexT cpt = 0;
  for (Landmarks::const_iterator it = sfm_data.GetLandmarks().begin();
      it != sfm_data.GetLandmarks().end(); ++it, ++cpt)
    {
      // Set it's normal to 0,0,0
      vec_nor[ cpt ] = Vec3(0,0,0);

      const Landmark & landmark = it->second;
      const Observations & obs = landmark.obs;
      // Sum all rays from all observations
      for (Observations::const_iterator itOb = obs.begin(); itOb != obs.end(); ++itOb )
        {
          const IndexT viewId = itOb->first;

          // Compute ray of this obs
          const Observation & ob = itOb->second;
          const Vec2 & pos = ob.x;

          const View * view = sfm_data.GetViews().at(viewId).get();
          if (!sfm_data.IsPoseAndIntrinsicDefined(view))
          {
            continue;
          }
          // get it's intrinsic (if available)
          Intrinsics::const_iterator iterIntrinsic = sfm_data.GetIntrinsics().find(view->id_intrinsic);
          if (!isPinhole(iterIntrinsic->second->getType()))
          {
            continue;
          }

          const Pose3 pose = sfm_data.GetPoseOrDie(view);

          // Force a pinhole ? (TODO : need to work with another type of camera)
          const Pinhole_Intrinsic * cam = dynamic_cast<const Pinhole_Intrinsic*>(iterIntrinsic->second.get());

          // Compute vector from camera center to the point
          const Vec3 C = pose.center();
          const Mat3 Kinv = cam->K().inverse();
          const Mat3 Rt = pose.rotation().transpose();

          // TODO : good for educational but really inefficient ! (+C-C)
          const Vec3 dst = Rt * ( ( Kinv * Vec3( pos.x() , pos.y() , 1.0 ) ) ) + C;
          const Vec3 n = ( dst - C ).normalized();
          vec_nor[ cpt ] += n;
        }
        // Mean and normalize
        vec_nor[ cpt ] = vec_nor[ cpt ].normalized();
    }
}

/// Find the color of the SfM_Data Landmarks/structure
bool ColorizeTracks(
  const SfM_Data & sfm_data,
  std::vector<Vec3> & vec_3dPoints,
  std::vector<Vec3> & vec_tracksColor)
{
  // Colorize each track
  //  Start with the most representative image
  //    and iterate to provide a color to each 3D point

  {
    C_Progress_display my_progress_bar(sfm_data.GetLandmarks().size(),
                                       std::cout,
                                       "\nCompute scene structure color\n");

    vec_tracksColor.resize(sfm_data.GetLandmarks().size());
    vec_3dPoints.resize(sfm_data.GetLandmarks().size());

    //Build a list of contiguous index for the trackIds
    std::map<IndexT, IndexT> trackIds_to_contiguousIndexes;
    IndexT cpt = 0;
    for (Landmarks::const_iterator it = sfm_data.GetLandmarks().begin();
      it != sfm_data.GetLandmarks().end(); ++it, ++cpt)
    {
      trackIds_to_contiguousIndexes[it->first] = cpt;
      vec_3dPoints[cpt] = it->second.X;
    }

    // The track list that will be colored (point removed during the process)
    std::set<IndexT> remainingTrackToColor;
    std::transform(sfm_data.GetLandmarks().begin(), sfm_data.GetLandmarks().end(),
      std::inserter(remainingTrackToColor, remainingTrackToColor.begin()),
      stl::RetrieveKey());

    while ( !remainingTrackToColor.empty() )
    {
      // Find the most representative image (for the remaining 3D points)
      //  a. Count the number of observation per view for each 3Dpoint Index
      //  b. Sort to find the most representative view index

      std::map<IndexT, IndexT> map_IndexCardinal; // ViewId, Cardinal
      for (std::set<IndexT>::const_iterator
        iterT = remainingTrackToColor.begin();
        iterT != remainingTrackToColor.end();
        ++iterT)
      {
        const size_t trackId = *iterT;
        const Observations & obs = sfm_data.GetLandmarks().at(trackId).obs;
        for ( Observations::const_iterator iterObs = obs.begin();
          iterObs != obs.end(); ++iterObs)
        {
          const size_t viewId = iterObs->first;
          if (map_IndexCardinal.find(viewId) == map_IndexCardinal.end())
            map_IndexCardinal[viewId] = 1;
          else
            ++map_IndexCardinal[viewId];
        }
      }

      // Find the View index that is the most represented
      std::vector<IndexT> vec_cardinal;
      std::transform(map_IndexCardinal.begin(),
        map_IndexCardinal.end(),
        std::back_inserter(vec_cardinal),
        stl::RetrieveValue());
      using namespace stl::indexed_sort;
      std::vector< sort_index_packet_descend< IndexT, IndexT>> packet_vec(vec_cardinal.size());
      sort_index_helper(packet_vec, &vec_cardinal[0], 1);

      // First image index with the most of occurence
      std::map<IndexT, IndexT>::const_iterator iterTT = map_IndexCardinal.begin();
      std::advance(iterTT, packet_vec[0].index);
      const size_t view_index = iterTT->first;
      const View * view = sfm_data.GetViews().at(view_index).get();
      const std::string sView_filename = stlplus::create_filespec(sfm_data.s_root_path,
        view->s_Img_path);
      Image<RGBColor> image_rgb;
      Image<unsigned char> image_gray;
      const bool b_rgb_image = ReadImage(sView_filename.c_str(), &image_rgb);
      if (!b_rgb_image) //try Gray level
      {
        const bool b_gray_image = ReadImage(sView_filename.c_str(), &image_gray);
        if (!b_gray_image)
        {
          std::cerr << "Cannot open provided the image." << std::endl;
          return false;
        }
      }

      // Iterate through the remaining track to color
      // - look if the current view is present to color the track
      std::set<IndexT> set_toRemove;
      for (std::set<IndexT>::const_iterator
        iterT = remainingTrackToColor.begin();
        iterT != remainingTrackToColor.end();
        ++iterT)
      {
        const size_t trackId = *iterT;
        const Observations & obs = sfm_data.GetLandmarks().at(trackId).obs;
        Observations::const_iterator it = obs.find(view_index);

        if (it != obs.end())
        {
          // Color the track
          const Vec2 & pt = it->second.x;
          const RGBColor color = b_rgb_image ? image_rgb(pt.y(), pt.x()) : RGBColor(image_gray(pt.y(), pt.x()));

          vec_tracksColor[ trackIds_to_contiguousIndexes[trackId] ] = Vec3(color.r(), color.g(), color.b());
          set_toRemove.insert(trackId);
          ++my_progress_bar;
        }
      }
      // Remove colored track
      for (std::set<IndexT>::const_iterator iter = set_toRemove.begin();
        iter != set_toRemove.end(); ++iter)
      {
        remainingTrackToColor.erase(*iter);
      }
    }
  }
  return true;
}

/**
* @brief Export scene data to a file
* @param sfm_data Input scene data
* @param sOutFile Output file name
*/
bool ExportSceneData( const SfM_Data & sfm_data, const std::string & sOutFile )
{
  std::ofstream file( sOutFile );
  if ( ! file )
  {
    std::cerr << std::endl;
    std::cerr << "Could not create : " << sOutFile << std::endl;
    return false;
  }

  std::vector<Vec3> vec_3dPoints, vec_tracksColor , vec_normals;
  ColorizeTracks(sfm_data, vec_3dPoints, vec_tracksColor);
  ComputeNormals(sfm_data,vec_normals);

  // Model points
  file << "modelPos=[" << std::endl;
  for (size_t id_point = 0; id_point < vec_3dPoints.size(); ++id_point)
  {
    file << vec_3dPoints[id_point](0) << " , " << vec_3dPoints[id_point](1) << " , " << vec_3dPoints[id_point](2);
    if (id_point +1 != vec_3dPoints.size() )<commit_msg>Removal of redundant ColorizeTracks function<commit_after>// This file is part of OpenMVG, an Open Multiple View Geometry C++ library.

// Copyright (c) 2017 <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/cameras/Camera_Pinhole.hpp"
#include "openMVG/geometry/pose3.hpp"
#include "openMVG/geometry/frustum.hpp"
#include "openMVG/image/image_io.hpp"
#include "openMVG/sfm/sfm_data.hpp"
#include "openMVG/sfm/sfm_data_io.hpp"
#include "openMVG/sfm/sfm_data_colorization.hpp"
#include "openMVG/stl/stl.hpp"

#include "config.h"

#include "third_party/cmdLine/cmdLine.h"
#include "third_party/progress/progress_display.hpp"
#include "third_party/stlplus3/filesystemSimplified/file_system.hpp"

#include <iostream>
#include <fstream>

using namespace openMVG;
using namespace openMVG::sfm;
using namespace openMVG::geometry;
using namespace openMVG::image;
using namespace openMVG::cameras;

/**
* @brief Compute normal using the mean of the direction for each points
* @param sfm_data Input scene data
* @param vec_nor A vector of normal for each point (in same order as ColorizeTrack output)
*/
void ComputeNormals( const SfM_Data & sfm_data , std::vector<Vec3> & vec_nor )
{
  vec_nor.resize( sfm_data.GetLandmarks().size() );

  IndexT cpt = 0;
  for (Landmarks::const_iterator it = sfm_data.GetLandmarks().begin();
      it != sfm_data.GetLandmarks().end(); ++it, ++cpt)
    {
      // Set it's normal to 0,0,0
      vec_nor[ cpt ] = Vec3(0,0,0);

      const Landmark & landmark = it->second;
      const Observations & obs = landmark.obs;
      // Sum all rays from all observations
      for (Observations::const_iterator itOb = obs.begin(); itOb != obs.end(); ++itOb )
        {
          const IndexT viewId = itOb->first;

          // Compute ray of this obs
          const Observation & ob = itOb->second;
          const Vec2 & pos = ob.x;

          const View * view = sfm_data.GetViews().at(viewId).get();
          if (!sfm_data.IsPoseAndIntrinsicDefined(view))
          {
            continue;
          }
          // get it's intrinsic (if available)
          Intrinsics::const_iterator iterIntrinsic = sfm_data.GetIntrinsics().find(view->id_intrinsic);
          if (!isPinhole(iterIntrinsic->second->getType()))
          {
            continue;
          }

          const Pose3 pose = sfm_data.GetPoseOrDie(view);

          // Force a pinhole ? (TODO : need to work with another type of camera)
          const Pinhole_Intrinsic * cam = dynamic_cast<const Pinhole_Intrinsic*>(iterIntrinsic->second.get());

          // Compute vector from camera center to the point
          const Vec3 C = pose.center();
          const Mat3 Kinv = cam->K().inverse();
          const Mat3 Rt = pose.rotation().transpose();

          // TODO : good for educational but really inefficient ! (+C-C)
          const Vec3 dst = Rt * ( ( Kinv * Vec3( pos.x() , pos.y() , 1.0 ) ) ) + C;
          const Vec3 n = ( dst - C ).normalized();
          vec_nor[ cpt ] += n;
        }
        // Mean and normalize
        vec_nor[ cpt ] = vec_nor[ cpt ].normalized();
    }
}

/**
* @brief Export scene data to a file
* @param sfm_data Input scene data
* @param sOutFile Output file name
*/
bool ExportSceneData( const SfM_Data & sfm_data, const std::string & sOutFile )
{
  std::ofstream file( sOutFile );
  if ( ! file )
  {
    std::cerr << std::endl;
    std::cerr << "Could not create : " << sOutFile << std::endl;
    return false;
  }

  std::vector<Vec3> vec_3dPoints, vec_tracksColor , vec_normals;
  ColorizeTracks(sfm_data, vec_3dPoints, vec_tracksColor);
  ComputeNormals(sfm_data,vec_normals);

  // Model points
  file << "modelPos=[" << std::endl;
  for (size_t id_point = 0; id_point < vec_3dPoints.size(); ++id_point)
  {
    file << vec_3dPoints[id_point](0) << " , " << vec_3dPoints[id_point](1) << " , " << vec_3dPoints[id_point](2);
    if (id_point +1 != vec_3dPoints.size() )
{'fraction_non_alphanumeric': 0.1263363246195864, 'fraction_numerical': 0.007725321888412017, 'mean_word_length': 2.8824598606482885, 'pattern_counts': {'":': 0, '<': 80, '<?xml version=': 0, '>': 52, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '254568', 'n_tokens_mistral': 4302, 'n_tokens_neox': 4143, 'n_words': 1034}