<commit_before>/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2009, Willow Garage, Inc.
 *  Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_
#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_

#include <pcl/sample_consensus/sac_model_line.h>
#include <pcl/common/centroid.h>
#include <pcl/common/concatenate.h>

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelLine<PointT>::isSampleGood (const std::vector<int> &samples) const
{
  if (
      (input_->points[samples[0]].x != input_->points[samples[1]].x)
    &&
      (input_->points[samples[0]].y != input_->points[samples[1]].y)
    &&
      (input_->points[samples[0]].z != input_->points[samples[1]].z))
    return (true);

  return (false);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelLine<PointT>::computeModelCoefficients (
      const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
{
  // Need 2 samples
  if (samples.size () != 2)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
    return (false);
  }

  if (fabs (input_->points[samples[0]].x - input_->points[samples[1]].x) <= std::numeric_limits<float>::epsilon () &&
      fabs (input_->points[samples[0]].y - input_->points[samples[1]].y) <= std::numeric_limits<float>::epsilon () &&
      fabs (input_->points[samples[0]].z - input_->points[samples[1]].z) <= std::numeric_limits<float>::epsilon ())
  {
    return (false);
  }

  model_coefficients.resize (6);
  model_coefficients[0] = input_->points[samples[0]].x;
  model_coefficients[1] = input_->points[samples[0]].y;
  model_coefficients[2] = input_->points[samples[0]].z;

  model_coefficients[3] = input_->points[samples[1]].x - model_coefficients[0];
  model_coefficients[4] = input_->points[samples[1]].y - model_coefficients[1];
  model_coefficients[5] = input_->points[samples[1]].z - model_coefficients[2];

  model_coefficients.template tail<3> ().normalize ();
  return (true);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::getDistancesToModel (
      const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
{
  // Needs a valid set of model coefficients
  if (!isModelValid (model_coefficients))
    return;

  distances.resize (indices_->size ());

  // Obtain the line point and direction
  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  line_dir.normalize ();

  // Iterate through the 3d points and calculate the distances from them to the line
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the line
    // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
    // Need to estimate sqrt here to keep MSAC and friends general
    distances[i] = sqrt ((line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ());
  }
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::selectWithinDistance (
      const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
  // Needs a valid set of model coefficients
  if (!isModelValid (model_coefficients))
    return;

  double sqr_threshold = threshold * threshold;

  int nr_p = 0;
  inliers.resize (indices_->size ());
  error_sqr_dists_.resize (indices_->size ());

  // Obtain the line point and direction
  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  line_dir.normalize ();

  // Iterate through the 3d points and calculate the distances from them to the line
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the line
    // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
    double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();

    if (sqr_distance < sqr_threshold)
    {
      // Returns the indices of the points whose squared distances are smaller than the threshold
      inliers[nr_p] = (*indices_)[i];
      error_sqr_dists_[nr_p] = sqr_distance;
      ++nr_p;
    }
  }
  inliers.resize (nr_p);
  error_sqr_dists_.resize (nr_p);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::SampleConsensusModelLine<PointT>::countWithinDistance (
      const Eigen::VectorXf &model_coefficients, const double threshold) const
{
  // Needs a valid set of model coefficients
  if (!isModelValid (model_coefficients))
    return (0);

  double sqr_threshold = threshold * threshold;

  int nr_p = 0;

  // Obtain the line point and direction
  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  line_dir.normalize ();

  // Iterate through the 3d points and calculate the distances from them to the line
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the line
    // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
    double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();

    if (sqr_distance < sqr_threshold)
      nr_p++;
  }
  return (nr_p);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::optimizeModelCoefficients (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
{
  // Needs a valid set of model coefficients
  if (!isModelValid (model_coefficients))
  {
    optimized_coefficients = model_coefficients;
    return;
  }

  // Need at least 2 points to estimate a line
  if (inliers.size () <= 2)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
    optimized_coefficients = model_coefficients;
    return;
  }

  optimized_coefficients.resize (6);

  // Compute the 3x3 covariance matrix
  Eigen::Vector4f centroid;
  compute3DCentroid (*input_, inliers, centroid);
  Eigen::Matrix3f covariance_matrix;
  computeCovarianceMatrix (*input_, inliers, centroid, covariance_matrix);
  optimized_coefficients[0] = centroid[0];
  optimized_coefficients[1] = centroid[1];
  optimized_coefficients[2] = centroid[2];

  // Extract the eigenvalues and eigenvectors
  EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
  pcl::eigen33 (covariance_matrix, eigen_values);
  pcl::computeCorrespondingEigenVector (covariance_matrix, eigen_values [2], eigen_vector);
  //pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);

  optimized_coefficients.template tail<3> ().matrix () = eigen_vector;
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::projectPoints (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
{
  // Needs a valid model coefficients
  if (!isModelValid (model_coefficients))
    return;

  // Obtain the line point and direction
  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);

  projected_points.header = input_->header;
  projected_points.is_dense = input_->is_dense;

  // Copy all the data fields from the input cloud to the projected one?
  if (copy_data_fields)
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (input_->points.size ());
    projected_points.width    = input_->width;
    projected_points.height   = input_->height;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < projected_points.points.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the line
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
      // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
      float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);

      Eigen::Vector4f pp = line_pt + k * line_dir;
      // Calculate the projection of the point on the line (pointProj = A + k * B)
      projected_points.points[inliers[i]].x = pp[0];
      projected_points.points[inliers[i]].y = pp[1];
      projected_points.points[inliers[i]].z = pp[2];
    }
  }
  else
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (inliers.size ());
    projected_points.width    = static_cast<uint32_t> (inliers.size ());
    projected_points.height   = 1;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < inliers.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the line
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
      // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
      float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);

      Eigen::Vector4f pp = line_pt + k * line_dir;
      // Calculate the projection of the point on the line (pointProj = A + k * B)
      projected_points.points[i].x = pp[0];
      projected_points.points[i].y = pp[1];
      projected_points.points[i].z = pp[2];
    }
  }
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelLine<PointT>::doSamplesVerifyModel (
      const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
{
  // Needs a valid set of model coefficients
  if (!isModelValid (model_coefficients))
    return (false);

  // Obtain the line point and direction
  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  line_dir.normalize ();

  double sqr_threshold = threshold * threshold;
  // Iterate through the 3d points and calculate the distances from them to the line
  for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
  {
    // Calculate the distance from the point to the line
    // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
    if ((line_pt - input_->points[*it].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold)
      return (false);
  }

  return (true);
}

#define PCL_INSTANTIATE_SampleConsensusModelLine(T) template class PCL_EXPORTS pcl::SampleConsensusModelLine<T>;

#endif    // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_

<commit_msg>Fix regression in pcl::SACSegmentation line fitting<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2009, Willow Garage, Inc.
 *  Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_
#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_

#include <pcl/sample_consensus/sac_model_line.h>
#include <pcl/common/centroid.h>
#include <pcl/common/concatenate.h>

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelLine<PointT>::isSampleGood (const std::vector<int> &samples) const
{
  // Make sure that the two sample points are not identical
  if (
      (input_->points[samples[0]].x != input_->points[samples[1]].x)
    ||
      (input_->points[samples[0]].y != input_->points[samples[1]].y)
    ||
      (input_->points[samples[0]].z != input_->points[samples[1]].z))
  {
    return (true);
  }

  return (false);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelLine<PointT>::computeModelCoefficients (
      const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
{
  // Need 2 samples
  if (samples.size () != 2)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelLine::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
    return (false);
  }

  if (fabs (input_->points[samples[0]].x - input_->points[samples[1]].x) <= std::numeric_limits<float>::epsilon () &&
      fabs (input_->points[samples[0]].y - input_->points[samples[1]].y) <= std::numeric_limits<float>::epsilon () &&
      fabs (input_->points[samples[0]].z - input_->points[samples[1]].z) <= std::numeric_limits<float>::epsilon ())
  {
    return (false);
  }

  model_coefficients.resize (6);
  model_coefficients[0] = input_->points[samples[0]].x;
  model_coefficients[1] = input_->points[samples[0]].y;
  model_coefficients[2] = input_->points[samples[0]].z;

  model_coefficients[3] = input_->points[samples[1]].x - model_coefficients[0];
  model_coefficients[4] = input_->points[samples[1]].y - model_coefficients[1];
  model_coefficients[5] = input_->points[samples[1]].z - model_coefficients[2];

  model_coefficients.template tail<3> ().normalize ();
  return (true);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::getDistancesToModel (
      const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
{
  // Needs a valid set of model coefficients
  if (!isModelValid (model_coefficients))
    return;

  distances.resize (indices_->size ());

  // Obtain the line point and direction
  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  line_dir.normalize ();

  // Iterate through the 3d points and calculate the distances from them to the line
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the line
    // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
    // Need to estimate sqrt here to keep MSAC and friends general
    distances[i] = sqrt ((line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ());
  }
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::selectWithinDistance (
      const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
  // Needs a valid set of model coefficients
  if (!isModelValid (model_coefficients))
    return;

  double sqr_threshold = threshold * threshold;

  int nr_p = 0;
  inliers.resize (indices_->size ());
  error_sqr_dists_.resize (indices_->size ());

  // Obtain the line point and direction
  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  line_dir.normalize ();

  // Iterate through the 3d points and calculate the distances from them to the line
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the line
    // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
    double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();

    if (sqr_distance < sqr_threshold)
    {
      // Returns the indices of the points whose squared distances are smaller than the threshold
      inliers[nr_p] = (*indices_)[i];
      error_sqr_dists_[nr_p] = sqr_distance;
      ++nr_p;
    }
  }
  inliers.resize (nr_p);
  error_sqr_dists_.resize (nr_p);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> int
pcl::SampleConsensusModelLine<PointT>::countWithinDistance (
      const Eigen::VectorXf &model_coefficients, const double threshold) const
{
  // Needs a valid set of model coefficients
  if (!isModelValid (model_coefficients))
    return (0);

  double sqr_threshold = threshold * threshold;

  int nr_p = 0;

  // Obtain the line point and direction
  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  line_dir.normalize ();

  // Iterate through the 3d points and calculate the distances from them to the line
  for (size_t i = 0; i < indices_->size (); ++i)
  {
    // Calculate the distance from the point to the line
    // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
    double sqr_distance = (line_pt - input_->points[(*indices_)[i]].getVector4fMap ()).cross3 (line_dir).squaredNorm ();

    if (sqr_distance < sqr_threshold)
      nr_p++;
  }
  return (nr_p);
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::optimizeModelCoefficients (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
{
  // Needs a valid set of model coefficients
  if (!isModelValid (model_coefficients))
  {
    optimized_coefficients = model_coefficients;
    return;
  }

  // Need at least 2 points to estimate a line
  if (inliers.size () <= 2)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelLine::optimizeModelCoefficients] Not enough inliers found to support a model (%lu)! Returning the same coefficients.\n", inliers.size ());
    optimized_coefficients = model_coefficients;
    return;
  }

  optimized_coefficients.resize (6);

  // Compute the 3x3 covariance matrix
  Eigen::Vector4f centroid;
  compute3DCentroid (*input_, inliers, centroid);
  Eigen::Matrix3f covariance_matrix;
  computeCovarianceMatrix (*input_, inliers, centroid, covariance_matrix);
  optimized_coefficients[0] = centroid[0];
  optimized_coefficients[1] = centroid[1];
  optimized_coefficients[2] = centroid[2];

  // Extract the eigenvalues and eigenvectors
  EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
  pcl::eigen33 (covariance_matrix, eigen_values);
  pcl::computeCorrespondingEigenVector (covariance_matrix, eigen_values [2], eigen_vector);
  //pcl::eigen33 (covariance_matrix, eigen_vectors, eigen_values);

  optimized_coefficients.template tail<3> ().matrix () = eigen_vector;
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::SampleConsensusModelLine<PointT>::projectPoints (
      const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields) const
{
  // Needs a valid model coefficients
  if (!isModelValid (model_coefficients))
    return;

  // Obtain the line point and direction
  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);

  projected_points.header = input_->header;
  projected_points.is_dense = input_->is_dense;

  // Copy all the data fields from the input cloud to the projected one?
  if (copy_data_fields)
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (input_->points.size ());
    projected_points.width    = input_->width;
    projected_points.height   = input_->height;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < projected_points.points.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the line
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
      // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
      float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);

      Eigen::Vector4f pp = line_pt + k * line_dir;
      // Calculate the projection of the point on the line (pointProj = A + k * B)
      projected_points.points[inliers[i]].x = pp[0];
      projected_points.points[inliers[i]].y = pp[1];
      projected_points.points[inliers[i]].z = pp[2];
    }
  }
  else
  {
    // Allocate enough space and copy the basics
    projected_points.points.resize (inliers.size ());
    projected_points.width    = static_cast<uint32_t> (inliers.size ());
    projected_points.height   = 1;

    typedef typename pcl::traits::fieldList<PointT>::type FieldList;
    // Iterate over each point
    for (size_t i = 0; i < inliers.size (); ++i)
      // Iterate over each dimension
      pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));

    // Iterate through the 3d points and calculate the distances from them to the line
    for (size_t i = 0; i < inliers.size (); ++i)
    {
      Eigen::Vector4f pt (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z, 0);
      // double k = (DOT_PROD_3D (points[i], p21) - dotA_B) / dotB_B;
      float k = (pt.dot (line_dir) - line_pt.dot (line_dir)) / line_dir.dot (line_dir);

      Eigen::Vector4f pp = line_pt + k * line_dir;
      // Calculate the projection of the point on the line (pointProj = A + k * B)
      projected_points.points[i].x = pp[0];
      projected_points.points[i].y = pp[1];
      projected_points.points[i].z = pp[2];
    }
  }
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT> bool
pcl::SampleConsensusModelLine<PointT>::doSamplesVerifyModel (
      const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const
{
  // Needs a valid set of model coefficients
  if (!isModelValid (model_coefficients))
    return (false);

  // Obtain the line point and direction
  Eigen::Vector4f line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
  Eigen::Vector4f line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
  line_dir.normalize ();

  double sqr_threshold = threshold * threshold;
  // Iterate through the 3d points and calculate the distances from them to the line
  for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
  {
    // Calculate the distance from the point to the line
    // D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
    if ((line_pt - input_->points[*it].getVector4fMap ()).cross3 (line_dir).squaredNorm () > sqr_threshold)
      return (false);
  }

  return (true);
}

#define PCL_INSTANTIATE_SampleConsensusModelLine(T) template class PCL_EXPORTS pcl::SampleConsensusModelLine<T>;

#endif    // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_LINE_H_

<|endoftext|>


<commit_before>#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sqlite3.h>
#include <getopt.h>
#include <string>
#include <vector>
#include <map>
#include <set>
#include <zlib.h>
#include <math.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <protozero/pbf_reader.hpp>
#include "mvt.hpp"
#include "projection.hpp"
#include "geometry.hpp"
#include "write_json.hpp"

int minzoom = 0;
int maxzoom = 32;
bool force = false;

void do_stats(mvt_tile &tile, size_t size, bool compressed, int z, unsigned x, unsigned y) {
	printf("{ \"zoom\": %d, \"x\": %u, \"y\": %u, \"bytes\": %zu, \"compressed\": %s", z, x, y, size, compressed ? "true" : "false");

	printf(", \"layers\": { ");
	for (size_t i = 0; i < tile.layers.size(); i++) {
		if (i != 0) {
			printf(", ");
		}
		fprintq(stdout, tile.layers[i].name.c_str());

		int points = 0, lines = 0, polygons = 0;
		for (size_t j = 0; j < tile.layers[i].features.size(); j++) {
			if (tile.layers[i].features[j].type == mvt_point) {
				points++;
			} else if (tile.layers[i].features[j].type == mvt_linestring) {
				lines++;
			} else if (tile.layers[i].features[j].type == mvt_polygon) {
				polygons++;
			}
		}

		printf(": { \"points\": %d, \"lines\": %d, \"polygons\": %d, \"extent\": %lld }", points, lines, polygons, tile.layers[i].extent);
	}

	printf(" } }\n");
}

void handle(std::string message, int z, unsigned x, unsigned y, int describe, std::set<std::string> const &to_decode, bool pipeline, bool stats) {
	mvt_tile tile;
	bool was_compressed;

	try {
		if (!tile.decode(message, was_compressed)) {
			fprintf(stderr, "Couldn't parse tile %d/%u/%u\n", z, x, y);
			exit(EXIT_FAILURE);
		}
	} catch (protozero::unknown_pbf_wire_type_exception e) {
		fprintf(stderr, "PBF decoding error in tile %d/%u/%u\n", z, x, y);
		exit(EXIT_FAILURE);
	}

	if (stats) {
		do_stats(tile, message.size(), was_compressed, z, x, y);
		return;
	}

	if (!pipeline) {
		printf("{ \"type\": \"FeatureCollection\"");

		if (describe) {
			printf(", \"properties\": { \"zoom\": %d, \"x\": %d, \"y\": %d", z, x, y);
			if (!was_compressed) {
				printf(", \"compressed\": false");
			}
			printf(" }");

			if (projection != projections) {
				printf(", \"crs\": { \"type\": \"name\", \"properties\": { \"name\": ");
				fprintq(stdout, projection->alias);
				printf(" } }");
			}
		}

		printf(", \"features\": [\n");
	}

	bool first_layer = true;
	for (size_t l = 0; l < tile.layers.size(); l++) {
		mvt_layer &layer = tile.layers[l];

		if (to_decode.size() != 0 && !to_decode.count(layer.name)) {
			continue;
		}

		if (!pipeline) {
			if (describe) {
				if (!first_layer) {
					printf(",\n");
				}

				printf("{ \"type\": \"FeatureCollection\"");
				printf(", \"properties\": { \"layer\": ");
				fprintq(stdout, layer.name.c_str());
				printf(", \"version\": %d, \"extent\": %lld", layer.version, layer.extent);
				printf(" }");
				printf(", \"features\": [\n");

				first_layer = false;
			}
		}

		layer_to_geojson(stdout, layer, z, x, y, !pipeline, pipeline, pipeline, 0, 0, 0, !force);

		if (!pipeline) {
			if (describe) {
				printf("] }\n");
			}
		}
	}

	if (!pipeline) {
		printf("] }\n");
	}
}

void decode(char *fname, int z, unsigned x, unsigned y, std::set<std::string> const &to_decode, bool pipeline, bool stats) {
	sqlite3 *db;
	int oz = z;
	unsigned ox = x, oy = y;

	int fd = open(fname, O_RDONLY | O_CLOEXEC);
	if (fd >= 0) {
		struct stat st;
		if (fstat(fd, &st) == 0) {
			if (st.st_size < 50 * 1024 * 1024) {
				char *map = (char *) mmap(NULL, st.st_size, PROT_READ, MAP_PRIVATE, fd, 0);
				if (map != NULL && map != MAP_FAILED) {
					if (strcmp(map, "SQLite format 3") != 0) {
						if (z >= 0) {
							std::string s = std::string(map, st.st_size);
							handle(s, z, x, y, 1, to_decode, pipeline, stats);
							munmap(map, st.st_size);
							return;
						} else {
							fprintf(stderr, "Must specify zoom/x/y to decode a single pbf file\n");
							exit(EXIT_FAILURE);
						}
					}
				}
				munmap(map, st.st_size);
			}
		} else {
			perror("fstat");
		}
		if (close(fd) != 0) {
			perror("close");
			exit(EXIT_FAILURE);
		}
	} else {
		perror(fname);
	}

	if (sqlite3_open(fname, &db) != SQLITE_OK) {
		fprintf(stderr, "%s: %s\n", fname, sqlite3_errmsg(db));
		exit(EXIT_FAILURE);
	}

	if (z < 0) {
		int within = 0;

		if (!pipeline && !stats) {
			printf("{ \"type\": \"FeatureCollection\", \"properties\": {\n");

			const char *sql2 = "SELECT name, value from metadata order by name;";
			sqlite3_stmt *stmt2;
			if (sqlite3_prepare_v2(db, sql2, -1, &stmt2, NULL) != SQLITE_OK) {
				fprintf(stderr, "%s: select failed: %s\n", fname, sqlite3_errmsg(db));
				exit(EXIT_FAILURE);
			}

			while (sqlite3_step(stmt2) == SQLITE_ROW) {
				if (within) {
					printf(",\n");
				}
				within = 1;

				const unsigned char *name = sqlite3_column_text(stmt2, 0);
				const unsigned char *value = sqlite3_column_text(stmt2, 1);

				if (name == NULL || value == NULL) {
					fprintf(stderr, "Corrupt mbtiles file: null metadata\n");
					exit(EXIT_FAILURE);
				}

				fprintq(stdout, (char *) name);
				printf(": ");
				fprintq(stdout, (char *) value);
			}

			sqlite3_finalize(stmt2);
		}

		if (stats) {
			printf("[\n");
		}

		const char *sql = "SELECT tile_data, zoom_level, tile_column, tile_row from tiles where zoom_level between ? and ? order by zoom_level, tile_column, tile_row;";
		sqlite3_stmt *stmt;
		if (sqlite3_prepare_v2(db, sql, -1, &stmt, NULL) != SQLITE_OK) {
			fprintf(stderr, "%s: select failed: %s\n", fname, sqlite3_errmsg(db));
			exit(EXIT_FAILURE);
		}

		sqlite3_bind_int(stmt, 1, minzoom);
		sqlite3_bind_int(stmt, 2, maxzoom);

		if (!pipeline && !stats) {
			printf("\n}, \"features\": [\n");
		}

		within = 0;
		while (sqlite3_step(stmt) == SQLITE_ROW) {
			if (!pipeline && !stats) {
				if (within) {
					printf(",\n");
				}
				within = 1;
			}
			if (stats) {
				if (within) {
					printf(",\n");
				}
				within = 1;
			}

			int len = sqlite3_column_bytes(stmt, 0);
			int tz = sqlite3_column_int(stmt, 1);
			int tx = sqlite3_column_int(stmt, 2);
			int ty = sqlite3_column_int(stmt, 3);
			ty = (1LL << tz) - 1 - ty;
			const char *s = (const char *) sqlite3_column_blob(stmt, 0);

			handle(std::string(s, len), tz, tx, ty, 1, to_decode, pipeline, stats);
		}

		if (!pipeline && !stats) {
			printf("] }\n");
		}
		if (stats) {
			printf("]\n");
		}

		sqlite3_finalize(stmt);
	} else {
		int handled = 0;
		while (z >= 0 && !handled) {
			const char *sql = "SELECT tile_data from tiles where zoom_level = ? and tile_column = ? and tile_row = ?;";
			sqlite3_stmt *stmt;
			if (sqlite3_prepare_v2(db, sql, -1, &stmt, NULL) != SQLITE_OK) {
				fprintf(stderr, "%s: select failed: %s\n", fname, sqlite3_errmsg(db));
				exit(EXIT_FAILURE);
			}

			sqlite3_bind_int(stmt, 1, z);
			sqlite3_bind_int(stmt, 2, x);
			sqlite3_bind_int(stmt, 3, (1LL << z) - 1 - y);

			while (sqlite3_step(stmt) == SQLITE_ROW) {
				int len = sqlite3_column_bytes(stmt, 0);
				const char *s = (const char *) sqlite3_column_blob(stmt, 0);

				if (z != oz) {
					fprintf(stderr, "%s: Warning: using tile %d/%u/%u instead of %d/%u/%u\n", fname, z, x, y, oz, ox, oy);
				}

				handle(std::string(s, len), z, x, y, 0, to_decode, pipeline, stats);
				handled = 1;
			}

			sqlite3_finalize(stmt);

			z--;
			x /= 2;
			y /= 2;
		}
	}

	if (sqlite3_close(db) != SQLITE_OK) {
		fprintf(stderr, "%s: could not close database: %s\n", fname, sqlite3_errmsg(db));
		exit(EXIT_FAILURE);
	}
}

void usage(char **argv) {
	fprintf(stderr, "Usage: %s [-s projection] [-Z minzoom] [-z maxzoom] [-l layer ...] file.mbtiles [zoom x y]\n", argv[0]);
	exit(EXIT_FAILURE);
}

int main(int argc, char **argv) {
	extern int optind;
	extern char *optarg;
	int i;
	std::set<std::string> to_decode;
	bool pipeline = false;
	bool stats = false;

	struct option long_options[] = {
		{"projection", required_argument, 0, 's'},
		{"maximum-zoom", required_argument, 0, 'z'},
		{"minimum-zoom", required_argument, 0, 'Z'},
		{"layer", required_argument, 0, 'l'},
		{"tag-layer-and-zoom", no_argument, 0, 'c'},
		{"stats", no_argument, 0, 'S'},
		{"force", no_argument, 0, 'f'},
		{0, 0, 0, 0},
	};

	std::string getopt_str;
	for (size_t lo = 0; long_options[lo].name != NULL; lo++) {
		if (long_options[lo].val > ' ') {
			getopt_str.push_back(long_options[lo].val);

			if (long_options[lo].has_arg == required_argument) {
				getopt_str.push_back(':');
			}
		}
	}

	while ((i = getopt_long(argc, argv, getopt_str.c_str(), long_options, NULL)) != -1) {
		switch (i) {
		case 0:
			break;

		case 's':
			set_projection_or_exit(optarg);
			break;

		case 'z':
			maxzoom = atoi(optarg);
			break;

		case 'Z':
			minzoom = atoi(optarg);
			break;

		case 'l':
			to_decode.insert(optarg);
			break;

		case 'c':
			pipeline = true;
			break;

		case 'S':
			stats = true;
			break;

		case 'f':
			force = true;
			break;

		default:
			usage(argv);
		}
	}

	if (argc == optind + 4) {
		decode(argv[optind], atoi(argv[optind + 1]), atoi(argv[optind + 2]), atoi(argv[optind + 3]), to_decode, pipeline, stats);
	} else if (argc == optind + 1) {
		decode(argv[optind], -1, -1, -1, to_decode, pipeline, stats);
	} else {
		usage(argv);
	}

	return 0;
}
<commit_msg>Guard against decoding tiles with an impossible extent<commit_after>#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sqlite3.h>
#include <getopt.h>
#include <string>
#include <vector>
#include <map>
#include <set>
#include <zlib.h>
#include <math.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <protozero/pbf_reader.hpp>
#include "mvt.hpp"
#include "projection.hpp"
#include "geometry.hpp"
#include "write_json.hpp"

int minzoom = 0;
int maxzoom = 32;
bool force = false;

void do_stats(mvt_tile &tile, size_t size, bool compressed, int z, unsigned x, unsigned y) {
	printf("{ \"zoom\": %d, \"x\": %u, \"y\": %u, \"bytes\": %zu, \"compressed\": %s", z, x, y, size, compressed ? "true" : "false");

	printf(", \"layers\": { ");
	for (size_t i = 0; i < tile.layers.size(); i++) {
		if (i != 0) {
			printf(", ");
		}
		fprintq(stdout, tile.layers[i].name.c_str());

		int points = 0, lines = 0, polygons = 0;
		for (size_t j = 0; j < tile.layers[i].features.size(); j++) {
			if (tile.layers[i].features[j].type == mvt_point) {
				points++;
			} else if (tile.layers[i].features[j].type == mvt_linestring) {
				lines++;
			} else if (tile.layers[i].features[j].type == mvt_polygon) {
				polygons++;
			}
		}

		printf(": { \"points\": %d, \"lines\": %d, \"polygons\": %d, \"extent\": %lld }", points, lines, polygons, tile.layers[i].extent);
	}

	printf(" } }\n");
}

void handle(std::string message, int z, unsigned x, unsigned y, int describe, std::set<std::string> const &to_decode, bool pipeline, bool stats) {
	mvt_tile tile;
	bool was_compressed;

	try {
		if (!tile.decode(message, was_compressed)) {
			fprintf(stderr, "Couldn't parse tile %d/%u/%u\n", z, x, y);
			exit(EXIT_FAILURE);
		}
	} catch (protozero::unknown_pbf_wire_type_exception e) {
		fprintf(stderr, "PBF decoding error in tile %d/%u/%u\n", z, x, y);
		exit(EXIT_FAILURE);
	}

	if (stats) {
		do_stats(tile, message.size(), was_compressed, z, x, y);
		return;
	}

	if (!pipeline) {
		printf("{ \"type\": \"FeatureCollection\"");

		if (describe) {
			printf(", \"properties\": { \"zoom\": %d, \"x\": %d, \"y\": %d", z, x, y);
			if (!was_compressed) {
				printf(", \"compressed\": false");
			}
			printf(" }");

			if (projection != projections) {
				printf(", \"crs\": { \"type\": \"name\", \"properties\": { \"name\": ");
				fprintq(stdout, projection->alias);
				printf(" } }");
			}
		}

		printf(", \"features\": [\n");
	}

	bool first_layer = true;
	for (size_t l = 0; l < tile.layers.size(); l++) {
		mvt_layer &layer = tile.layers[l];

		if (layer.extent <= 0) {
			fprintf(stderr, "Impossible layer extent %lld in mbtiles\n", layer.extent);
			exit(EXIT_FAILURE);
		}

		if (to_decode.size() != 0 && !to_decode.count(layer.name)) {
			continue;
		}

		if (!pipeline) {
			if (describe) {
				if (!first_layer) {
					printf(",\n");
				}

				printf("{ \"type\": \"FeatureCollection\"");
				printf(", \"properties\": { \"layer\": ");
				fprintq(stdout, layer.name.c_str());
				printf(", \"version\": %d, \"extent\": %lld", layer.version, layer.extent);
				printf(" }");
				printf(", \"features\": [\n");

				first_layer = false;
			}
		}

		layer_to_geojson(stdout, layer, z, x, y, !pipeline, pipeline, pipeline, 0, 0, 0, !force);

		if (!pipeline) {
			if (describe) {
				printf("] }\n");
			}
		}
	}

	if (!pipeline) {
		printf("] }\n");
	}
}

void decode(char *fname, int z, unsigned x, unsigned y, std::set<std::string> const &to_decode, bool pipeline, bool stats) {
	sqlite3 *db;
	int oz = z;
	unsigned ox = x, oy = y;

	int fd = open(fname, O_RDONLY | O_CLOEXEC);
	if (fd >= 0) {
		struct stat st;
		if (fstat(fd, &st) == 0) {
			if (st.st_size < 50 * 1024 * 1024) {
				char *map = (char *) mmap(NULL, st.st_size, PROT_READ, MAP_PRIVATE, fd, 0);
				if (map != NULL && map != MAP_FAILED) {
					if (strcmp(map, "SQLite format 3") != 0) {
						if (z >= 0) {
							std::string s = std::string(map, st.st_size);
							handle(s, z, x, y, 1, to_decode, pipeline, stats);
							munmap(map, st.st_size);
							return;
						} else {
							fprintf(stderr, "Must specify zoom/x/y to decode a single pbf file\n");
							exit(EXIT_FAILURE);
						}
					}
				}
				munmap(map, st.st_size);
			}
		} else {
			perror("fstat");
		}
		if (close(fd) != 0) {
			perror("close");
			exit(EXIT_FAILURE);
		}
	} else {
		perror(fname);
	}

	if (sqlite3_open(fname, &db) != SQLITE_OK) {
		fprintf(stderr, "%s: %s\n", fname, sqlite3_errmsg(db));
		exit(EXIT_FAILURE);
	}

	if (z < 0) {
		int within = 0;

		if (!pipeline && !stats) {
			printf("{ \"type\": \"FeatureCollection\", \"properties\": {\n");

			const char *sql2 = "SELECT name, value from metadata order by name;";
			sqlite3_stmt *stmt2;
			if (sqlite3_prepare_v2(db, sql2, -1, &stmt2, NULL) != SQLITE_OK) {
				fprintf(stderr, "%s: select failed: %s\n", fname, sqlite3_errmsg(db));
				exit(EXIT_FAILURE);
			}

			while (sqlite3_step(stmt2) == SQLITE_ROW) {
				if (within) {
					printf(",\n");
				}
				within = 1;

				const unsigned char *name = sqlite3_column_text(stmt2, 0);
				const unsigned char *value = sqlite3_column_text(stmt2, 1);

				if (name == NULL || value == NULL) {
					fprintf(stderr, "Corrupt mbtiles file: null metadata\n");
					exit(EXIT_FAILURE);
				}

				fprintq(stdout, (char *) name);
				printf(": ");
				fprintq(stdout, (char *) value);
			}

			sqlite3_finalize(stmt2);
		}

		if (stats) {
			printf("[\n");
		}

		const char *sql = "SELECT tile_data, zoom_level, tile_column, tile_row from tiles where zoom_level between ? and ? order by zoom_level, tile_column, tile_row;";
		sqlite3_stmt *stmt;
		if (sqlite3_prepare_v2(db, sql, -1, &stmt, NULL) != SQLITE_OK) {
			fprintf(stderr, "%s: select failed: %s\n", fname, sqlite3_errmsg(db));
			exit(EXIT_FAILURE);
		}

		sqlite3_bind_int(stmt, 1, minzoom);
		sqlite3_bind_int(stmt, 2, maxzoom);

		if (!pipeline && !stats) {
			printf("\n}, \"features\": [\n");
		}

		within = 0;
		while (sqlite3_step(stmt) == SQLITE_ROW) {
			if (!pipeline && !stats) {
				if (within) {
					printf(",\n");
				}
				within = 1;
			}
			if (stats) {
				if (within) {
					printf(",\n");
				}
				within = 1;
			}

			int len = sqlite3_column_bytes(stmt, 0);
			int tz = sqlite3_column_int(stmt, 1);
			int tx = sqlite3_column_int(stmt, 2);
			int ty = sqlite3_column_int(stmt, 3);
			ty = (1LL << tz) - 1 - ty;
			const char *s = (const char *) sqlite3_column_blob(stmt, 0);

			handle(std::string(s, len), tz, tx, ty, 1, to_decode, pipeline, stats);
		}

		if (!pipeline && !stats) {
			printf("] }\n");
		}
		if (stats) {
			printf("]\n");
		}

		sqlite3_finalize(stmt);
	} else {
		int handled = 0;
		while (z >= 0 && !handled) {
			const char *sql = "SELECT tile_data from tiles where zoom_level = ? and tile_column = ? and tile_row = ?;";
			sqlite3_stmt *stmt;
			if (sqlite3_prepare_v2(db, sql, -1, &stmt, NULL) != SQLITE_OK) {
				fprintf(stderr, "%s: select failed: %s\n", fname, sqlite3_errmsg(db));
				exit(EXIT_FAILURE);
			}

			sqlite3_bind_int(stmt, 1, z);
			sqlite3_bind_int(stmt, 2, x);
			sqlite3_bind_int(stmt, 3, (1LL << z) - 1 - y);

			while (sqlite3_step(stmt) == SQLITE_ROW) {
				int len = sqlite3_column_bytes(stmt, 0);
				const char *s = (const char *) sqlite3_column_blob(stmt, 0);

				if (z != oz) {
					fprintf(stderr, "%s: Warning: using tile %d/%u/%u instead of %d/%u/%u\n", fname, z, x, y, oz, ox, oy);
				}

				handle(std::string(s, len), z, x, y, 0, to_decode, pipeline, stats);
				handled = 1;
			}

			sqlite3_finalize(stmt);

			z--;
			x /= 2;
			y /= 2;
		}
	}

	if (sqlite3_close(db) != SQLITE_OK) {
		fprintf(stderr, "%s: could not close database: %s\n", fname, sqlite3_errmsg(db));
		exit(EXIT_FAILURE);
	}
}

void usage(char **argv) {
	fprintf(stderr, "Usage: %s [-s projection] [-Z minzoom] [-z maxzoom] [-l layer ...] file.mbtiles [zoom x y]\n", argv[0]);
	exit(EXIT_FAILURE);
}

int main(int argc, char **argv) {
	extern int optind;
	extern char *optarg;
	int i;
	std::set<std::string> to_decode;
	bool pipeline = false;
	bool stats = false;

	struct option long_options[] = {
		{"projection", required_argument, 0, 's'},
		{"maximum-zoom", required_argument, 0, 'z'},
		{"minimum-zoom", required_argument, 0, 'Z'},
		{"layer", required_argument, 0, 'l'},
		{"tag-layer-and-zoom", no_argument, 0, 'c'},
		{"stats", no_argument, 0, 'S'},
		{"force", no_argument, 0, 'f'},
		{0, 0, 0, 0},
	};

	std::string getopt_str;
	for (size_t lo = 0; long_options[lo].name != NULL; lo++) {
		if (long_options[lo].val > ' ') {
			getopt_str.push_back(long_options[lo].val);

			if (long_options[lo].has_arg == required_argument) {
				getopt_str.push_back(':');
			}
		}
	}

	while ((i = getopt_long(argc, argv, getopt_str.c_str(), long_options, NULL)) != -1) {
		switch (i) {
		case 0:
			break;

		case 's':
			set_projection_or_exit(optarg);
			break;

		case 'z':
			maxzoom = atoi(optarg);
			break;

		case 'Z':
			minzoom = atoi(optarg);
			break;

		case 'l':
			to_decode.insert(optarg);
			break;

		case 'c':
			pipeline = true;
			break;

		case 'S':
			stats = true;
			break;

		case 'f':
			force = true;
			break;

		default:
			usage(argv);
		}
	}

	if (argc == optind + 4) {
		decode(argv[optind], atoi(argv[optind + 1]), atoi(argv[optind + 2]), atoi(argv[optind + 3]), to_decode, pipeline, stats);
	} else if (argc == optind + 1) {
		decode(argv[optind], -1, -1, -1, to_decode, pipeline, stats);
	} else {
		usage(argv);
	}

	return 0;
}
<|endoftext|>


<commit_before><commit_msg>Remove an unused declaration.<commit_after><|endoftext|>


<commit_before>#include "arguments.h"
#include "../utils/version.h"
#include <iostream>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>

using namespace thewizardplusplus::language_do::arguments;
using namespace thewizardplusplus::language_do::utils;
using namespace boost;
using namespace boost::program_options;
using namespace boost::filesystem;

const auto POSITIONAL_ARGUMENT_SIGNLE_REPETITION = 1;
const auto POSITIONAL_ARGUMENT_UNLIMITED_REPETITIONS = -1;

namespace {

auto MakeArgumentsDescription() -> options_description;
auto ParseArguments(
	const std::vector<std::string>& arguments,
	const options_description& arguments_description
) -> variables_map;
auto MakePositionalArgumentsDescription() -> positional_options_description;
auto MakeParser(
	const std::vector<std::string>& arguments,
	const options_description& arguments_description,
	const positional_options_description& positional_arguments_description
) -> command_line_parser;
auto RunParser(command_line_parser& parser) -> variables_map;

auto ProcessVersionOption(const variables_map& arguments) -> void;
auto ProcessHelpOption(
	const variables_map& arguments,
	const options_description& arguments_description
) -> void;
auto GetParameters(
	const std::vector<std::string>& original_arguments,
	const variables_map& parsed_arguments
) -> Parameters;
auto GetInterpreterPathOption(
	const std::vector<std::string>& arguments
) -> std::string;
auto GetFinalStageOption(const variables_map& arguments) -> FinalStage;
auto GetOutputFileOption(const variables_map& arguments) -> std::string;
auto GetUseOutputOption(const variables_map& arguments) -> bool;
auto GetScriptFileOption(const variables_map& arguments) -> std::string;
auto GetScriptArgumentOptions(
	const variables_map& arguments
) -> std::vector<std::string>;

}

namespace thewizardplusplus {
namespace language_do {
namespace arguments {

auto ProcessArguments(const std::vector<std::string>& arguments) -> Parameters {
	const auto arguments_description = MakeArgumentsDescription();
	const auto parsed_arguments = ParseArguments(
		arguments,
		arguments_description
	);

	ProcessVersionOption(parsed_arguments);
	ProcessHelpOption(parsed_arguments, arguments_description);

	return GetParameters(arguments, parsed_arguments);
}

}
}
}

namespace {

auto MakeArgumentsDescription() -> options_description {
	auto description = options_description("Options");
	description.add_options()
		("version,v", "- show version;")
		("help,h", "- show help;")
		(
			"final-stage,f",
			value<std::string>(),
			R"(- set final stage (allowed: "code", "ast" and "c");)"
		)
		(
			"output-file,o",
			value<std::string>(),
			"- output file for compiling result;"
		)
		("use-output,u", "- output any final stage result to output file;")
		("script-file", value<std::string>(), "- script file;")
		(
			"script-argument",
			value<std::vector<std::string>>()->composing(),
			"- script argument."
		);

	return description;
}

auto ParseArguments(
	const std::vector<std::string>& arguments,
	const options_description& arguments_description
) -> variables_map {
	// должно быть создано здесь, т. к. время жизни должно включать время работы
	// RunParser()
	const auto positional_arguments_description =
		MakePositionalArgumentsDescription();
	auto parser = MakeParser(
		arguments,
		arguments_description,
		positional_arguments_description
	);
	return RunParser(parser);
}

auto MakePositionalArgumentsDescription() -> positional_options_description {
	return
		positional_options_description()
		.add("script-file", POSITIONAL_ARGUMENT_SIGNLE_REPETITION)
		.add("script-argument", POSITIONAL_ARGUMENT_UNLIMITED_REPETITIONS);
}

auto MakeParser(
	const std::vector<std::string>& arguments,
	const options_description& arguments_description,
	const positional_options_description& positional_arguments_description
) -> command_line_parser {
	return
		command_line_parser(
			std::vector<std::string>(
				std::begin(arguments) + 1,
				std::end(arguments)
			)
		)
		.options(arguments_description)
		.positional(positional_arguments_description);
}

auto RunParser(command_line_parser& parser) -> variables_map {
	auto arguments = variables_map();
	const auto result = parser.run();
	store(result, arguments);

	notify(arguments);
	return arguments;
}

auto ProcessVersionOption(const variables_map& arguments) -> void {
	if (!arguments.count("version")) {
		return;
	}

	std::cout
		<< "Do interpreter, " << VERSION_STRING << "\n"
		<< "(c) thewizardplusplus, 2015\n";
	std::exit(EXIT_SUCCESS);
}

auto ProcessHelpOption(
	const variables_map& arguments,
	const options_description& arguments_description
) -> void {
	if (!arguments.count("help")) {
		return;
	}

	std::cout
		<< "Usage:\n"
		<< "  doi [options] <script-file> [<script-argument>...]\n"
		<< "\n"
		<< arguments_description;
	std::exit(EXIT_SUCCESS);
}

auto GetParameters(
	const std::vector<std::string>& original_arguments,
	const variables_map& parsed_arguments
) -> Parameters {
	auto parameters = Parameters();
	parameters.interpreter_path = GetInterpreterPathOption(original_arguments);
	parameters.final_stage = GetFinalStageOption(parsed_arguments);
	parameters.output_file = GetOutputFileOption(parsed_arguments);
	parameters.use_output = GetUseOutputOption(parsed_arguments);
	parameters.script_file = GetScriptFileOption(parsed_arguments);
	parameters.script_arguments = GetScriptArgumentOptions(parsed_arguments);

	return parameters;
}

auto GetInterpreterPathOption(
	const std::vector<std::string>& arguments
) -> std::string {
	return path(arguments[0]).parent_path().string();
}

auto GetFinalStageOption(const variables_map& arguments) -> FinalStage {
	if (!arguments.count("final-stage")) {
		return FinalStage::NONE;
	}

	auto final_stage = FinalStage::NONE;
	const auto string_final_stage = arguments["final-stage"].as<std::string>();
	if (string_final_stage == "code") {
		final_stage = FinalStage::CODE;
	} else if (string_final_stage == "ast") {
		final_stage = FinalStage::AST;
	} else if (string_final_stage == "c") {
		final_stage = FinalStage::C;
	} else {
		throw std::runtime_error(
			(format(R"(unknown final stage "%s")") % string_final_stage).str()
		);
	}

	return final_stage;
}

auto GetOutputFileOption(const variables_map& arguments) -> std::string {
	if (!arguments.count("output-file")) {
		return "";
	}

	return arguments["output-file"].as<std::string>();
}

auto GetUseOutputOption(const variables_map& arguments) -> bool {
	return arguments.count("use-output") && arguments.count("output-file");
}

auto GetScriptFileOption(const variables_map& arguments) -> std::string {
	if (!arguments.count("script-file")) {
		throw std::runtime_error("script file not specified");
	}

	return arguments["script-file"].as<std::string>();
}

auto GetScriptArgumentOptions(
	const variables_map& arguments
) -> std::vector<std::string> {
	if (!arguments.count("script-argument")) {
		return {};
	}

	return arguments["script-argument"].as<std::vector<std::string>>();
}

}
<commit_msg>Fixes #34: restore simple code.<commit_after>#include "arguments.h"
#include "../utils/version.h"
#include <iostream>
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>

using namespace thewizardplusplus::language_do::arguments;
using namespace thewizardplusplus::language_do::utils;
using namespace boost;
using namespace boost::program_options;
using namespace boost::filesystem;

const auto POSITIONAL_ARGUMENT_SIGNLE_REPETITION = 1;
const auto POSITIONAL_ARGUMENT_UNLIMITED_REPETITIONS = -1;

namespace thewizardplusplus {
namespace language_do {
namespace arguments {

auto ProcessArguments(const std::vector<std::string>& arguments) -> Parameters {
	auto arguments_description = options_description("Options");
	arguments_description.add_options()
		("version,v", "- show version;")
		("help,h", "- show help;")
		(
			"final-stage,f",
			value<std::string>(),
			R"(- set final stage (allowed: "code", "ast" and "c");)"
		)
		(
			"output-file,o",
			value<std::string>(),
			"- output file for compiling result;"
		)
		("use-output,u", "- output any final stage result to output file;")
		("script-file", value<std::string>(), "- script file;")
		(
			"script-argument",
			value<std::vector<std::string>>()->composing(),
			"- script argument."
		);
	const auto positional_arguments_description =
		positional_options_description()
		.add("script-file", POSITIONAL_ARGUMENT_SIGNLE_REPETITION)
		.add("script-argument", POSITIONAL_ARGUMENT_UNLIMITED_REPETITIONS);

	auto parsed_arguments = variables_map();
	const auto parsing_result =
		command_line_parser(
			std::vector<std::string>(
				std::begin(arguments) + 1,
				std::end(arguments)
			)
		)
		.options(arguments_description)
		.positional(positional_arguments_description)
		.run();
	store(parsing_result, parsed_arguments);
	notify(parsed_arguments);

	if (parsed_arguments.count("version")) {
		std::cout
			<< "Do interpreter, " << VERSION_STRING << "\n"
			<< "(c) thewizardplusplus, 2015\n";
		std::exit(EXIT_SUCCESS);
	}
	if (parsed_arguments.count("help")) {
		std::cout
			<< "Usage:\n"
			<< "  doi [options] <script-file> [<script-argument>...]\n"
			<< "\n"
			<< arguments_description;
		std::exit(EXIT_SUCCESS);
	}

	auto parameters = Parameters();
	parameters.interpreter_path = path(arguments[0]).parent_path().string();
	if (parsed_arguments.count("final-stage")) {
		const auto final_stage =
			parsed_arguments["final-stage"]
			.as<std::string>();
		if (final_stage == "code") {
			parameters.final_stage = FinalStage::CODE;
		} else if (final_stage == "ast") {
			parameters.final_stage = FinalStage::AST;
		} else if (final_stage == "c") {
			parameters.final_stage = FinalStage::C;
		} else {
			throw std::runtime_error(
				(format(R"(unknown final stage "%s")") % final_stage).str()
			);
		}
	}
	if (parsed_arguments.count("output-file")) {
		parameters.output_file =
			parsed_arguments["output-file"]
			.as<std::string>();
	}
	parameters.use_output =
		parsed_arguments.count("use-output")
		&& parsed_arguments.count("output-file");
	if (parsed_arguments.count("script-file")) {
		parameters.script_file =
			parsed_arguments["script-file"]
			.as<std::string>();
	} else {
		throw std::runtime_error("script file not specified");
	}
	if (parsed_arguments.count("script-argument")) {
		parameters.script_arguments =
			parsed_arguments["script-argument"]
			.as<std::vector<std::string>>();
	}

	return parameters;
}

}
}
}
<|endoftext|>


<commit_before>#include "ImageLoader.h"
#include "Utilities/ResourceManager.h"

namespace Graphics
{

namespace ImageLoader
{

SDL_Surface* LoadImage ( const std::string& path )
{
	SDL_RWops* rwops = ResourceManager::OpenFile(path);
	if (!rwops)
		return NULL;
	SDL_Surface* surface = IMG_LoadTyped_RW(rwops, 1, const_cast<char*>("PNG"));
	return surface;
}

GLuint CreateTexture ( SDL_Surface* surface, bool autofree, bool rectangle )
{
	GLuint texID;
	glGenTextures(1, &texID);
	GLenum target = rectangle ? GL_TEXTURE_RECTANGLE_ARB : GL_TEXTURE_2D;
	glBindTexture(target, texID);
	if (surface->format->BytesPerPixel == 3)
	{
		glTexImage2D(target, 0, GL_RGB,
					 surface->w, surface->h, 0,
					 GL_RGB, GL_UNSIGNED_BYTE, surface->pixels);
	}
	else
	{
		glTexImage2D(target, 0, GL_RGBA,
					 surface->w, surface->h, 0,
					 GL_RGBA, GL_UNSIGNED_BYTE, surface->pixels);
	}
	if (autofree)
		SDL_FreeSurface(surface);
	return texID;
}

}

}
<commit_msg>New SDL fully implemented with correct colors.<commit_after>#include "ImageLoader.h"
#include "Utilities/ResourceManager.h"

namespace Graphics
{

namespace ImageLoader
{

SDL_Surface* LoadImage ( const std::string& path )
{
	SDL_RWops* rwops = ResourceManager::OpenFile(path);
	if (!rwops)
		return NULL;
	SDL_Surface* surface = IMG_LoadTyped_RW(rwops, 1, const_cast<char*>("PNG"));
	return surface;
}

GLuint CreateTexture ( SDL_Surface* surface, bool autofree, bool rectangle )
{
	GLuint texID;
	glGenTextures(1, &texID);
	GLenum target = rectangle ? GL_TEXTURE_RECTANGLE_ARB : GL_TEXTURE_2D;
	glBindTexture(target, texID);
	if (surface->format->BytesPerPixel == 3)
	{
		GLenum format = (surface->format->Rmask) != 0xFF ? GL_BGR_EXT : GL_RGB;
		glTexImage2D(target, 0, GL_RGB,
					 surface->w, surface->h, 0,
					 format, GL_UNSIGNED_BYTE, surface->pixels);
	}
	else
	{
		GLenum format = (surface->format->Rmask) != 0xFF ? GL_BGRA_EXT : GL_RGBA;
		glTexImage2D(target, 0, GL_RGBA,
					 surface->w, surface->h, 0,
					 format, GL_UNSIGNED_BYTE, surface->pixels);
	}
	if (autofree)
		SDL_FreeSurface(surface);
	return texID;
}

}

}
<|endoftext|>


<commit_before>////////////////////////////////////////////////////////////////////////////////
// taskwarrior - a command line task list manager.
//
// Copyright 2006 - 2011, Paul Beckingham, Federico Hernandez.
// All rights reserved.
//
// This program is free software; you can redistribute it and/or modify it under
// the terms of the GNU General Public License as published by the Free Software
// Foundation; either version 2 of the License, or (at your option) any later
// version.
//
// This program is distributed in the hope that it will be useful, but WITHOUT
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
// details.
//
// You should have received a copy of the GNU General Public License along with
// this program; if not, write to the
//
//     Free Software Foundation, Inc.,
//     51 Franklin Street, Fifth Floor,
//     Boston, MA
//     02110-1301
//     USA
//
////////////////////////////////////////////////////////////////////////////////
#include <iostream>
#include <unistd.h>
#include <cmake.h>
#include <main.h>
#include <test.h>

#ifdef HAVE_LIBLUA
extern "C"
{
  #include <lua.h>
}
#endif

Context context;

////////////////////////////////////////////////////////////////////////////////
int main (int argc, char** argv)
{
  UnitTest t (7);

  try
  {
    DOM dom;

    // TODO dom.get rc.name
    t.is (dom.get ("system.version"),     VERSION,     "DOM system.version -> VERSION");
    t.is (dom.get ("system.lua.version"), LUA_RELEASE, "DOM system.lua.version -> LUA_RELEASE");
    t.ok (dom.get ("system.os") != "<unknown>",        "DOM system.os -> != Unknown");
    t.is (dom.get ("context.program"),    "",          "DOM context.program -> ''");
    t.is (dom.get ("context.args"),       "",          "DOM context.args -> ''");
    t.is (dom.get ("context.width"),      "0",         "DOM context.width -> '0'");
    t.is (dom.get ("context.height"),     "0",         "DOM context.height -> '0'");

    // TODO dom.set rc.name
  }

  catch (std::string& error)
  {
    t.diag (error);
    return -1;
  }

  catch (...)
  {
    t.diag ("Unknown error.");
    return -2;
  }

  return 0;
}

////////////////////////////////////////////////////////////////////////////////

<commit_msg>Unit Tests<commit_after>////////////////////////////////////////////////////////////////////////////////
// taskwarrior - a command line task list manager.
//
// Copyright 2006 - 2011, Paul Beckingham, Federico Hernandez.
// All rights reserved.
//
// This program is free software; you can redistribute it and/or modify it under
// the terms of the GNU General Public License as published by the Free Software
// Foundation; either version 2 of the License, or (at your option) any later
// version.
//
// This program is distributed in the hope that it will be useful, but WITHOUT
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
// FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
// details.
//
// You should have received a copy of the GNU General Public License along with
// this program; if not, write to the
//
//     Free Software Foundation, Inc.,
//     51 Franklin Street, Fifth Floor,
//     Boston, MA
//     02110-1301
//     USA
//
////////////////////////////////////////////////////////////////////////////////
#include <iostream>
#include <unistd.h>
#include <cmake.h>
#include <main.h>
#include <test.h>

#ifdef HAVE_LIBLUA
extern "C"
{
  #include <lua.h>
}
#endif

Context context;

////////////////////////////////////////////////////////////////////////////////
int main (int argc, char** argv)
{
  UnitTest t (7);

  try
  {
    // Prime the pump.
    context.args.capture ("task");

    // TODO dom.get rc.name
    DOM dom;
    t.is (dom.get ("system.version"),     VERSION,     "DOM system.version -> VERSION");
    t.is (dom.get ("system.lua.version"), LUA_RELEASE, "DOM system.lua.version -> LUA_RELEASE");
    t.ok (dom.get ("system.os") != "<unknown>",        "DOM system.os -> != Unknown");
    t.is (dom.get ("context.program"),    "task",      "DOM context.program -> 'task'");
    t.is (dom.get ("context.args"),       "task",      "DOM context.args -> 'task'");
    t.is (dom.get ("context.width"),      "0",         "DOM context.width -> '0'");
    t.is (dom.get ("context.height"),     "0",         "DOM context.height -> '0'");

    // TODO dom.set rc.name
  }

  catch (std::string& error)
  {
    t.diag (error);
    return -1;
  }

  catch (...)
  {
    t.diag ("Unknown error.");
    return -2;
  }

  return 0;
}

////////////////////////////////////////////////////////////////////////////////

<|endoftext|>


<commit_before>// Copyright 2022 Google LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     https://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "google/cloud/pubsublite/internal/multipartition_publisher.h"
#include <functional>
#include <limits>

namespace google {
namespace cloud {
namespace pubsublite_internal {
GOOGLE_CLOUD_CPP_INLINE_NAMESPACE_BEGIN

using ::google::cloud::pubsublite::AdminServiceConnection;
using ::google::cloud::pubsublite::MessageMetadata;
using ::google::cloud::pubsublite::Topic;
using ::google::cloud::pubsublite::v1::Cursor;
using ::google::cloud::pubsublite::v1::PubSubMessage;
using ::google::cloud::pubsublite::v1::TopicPartitions;

MultipartitionPublisher::MultipartitionPublisher(
    PartitionPublisherFactory publisher_factory,
    std::shared_ptr<AdminServiceConnection> admin_connection,
    AlarmRegistry& alarm_registry,
    std::unique_ptr<RoutingPolicy> routing_policy, Topic topic)
    : publisher_factory_{std::move(publisher_factory)},
      admin_connection_{std::move(admin_connection)},
      routing_policy_{std::move(routing_policy)},
      topic_{std::move(topic)},
      topic_partitions_request_{[&] {
        google::cloud::pubsublite::v1::GetTopicPartitionsRequest request;
        *request.mutable_name() = topic_.FullName();
        return request;
      }()},
      cancel_token_{alarm_registry.RegisterAlarm(
          std::chrono::seconds{60}, [this]() { TriggerPublisherCreation(); })} {
}

MultipartitionPublisher::~MultipartitionPublisher() {
  future<void> shutdown = Shutdown();
  if (!shutdown.is_ready()) {
    GCP_LOG(WARNING) << "`Shutdown` must be called and finished before object "
                        "goes out of scope.";
    assert(false);
  }
  shutdown.get();
}

future<Status> MultipartitionPublisher::Start() {
  auto start = service_composite_.Start();
  TriggerPublisherCreation();
  return start;
}

future<StatusOr<std::uint32_t>> MultipartitionPublisher::GetNumPartitions() {
  return admin_connection_->AsyncGetTopicPartitions(topic_partitions_request_)
      .then([](future<StatusOr<TopicPartitions>> f) -> StatusOr<std::uint32_t> {
        auto partitions = f.get();
        if (!partitions) return std::move(partitions).status();
        if (partitions->partition_count() >=
            std::numeric_limits<std::uint32_t>::max()) {
          return Status{StatusCode::kInternal,
                        absl::StrCat("Returned partition count is too big: ",
                                     partitions->partition_count())};
        }
        return static_cast<std::uint32_t>(partitions->partition_count());
      });
}

void MultipartitionPublisher::HandleNumPartitions(
    std::uint32_t num_partitions) {
  std::uint32_t current_num_partitions;
  {
    std::lock_guard<std::mutex> g{mu_};
    current_num_partitions =
        static_cast<std::uint32_t>(partition_publishers_.size());
  }
  assert(num_partitions >= current_num_partitions);  // should be no race
  if (num_partitions == current_num_partitions) return;
  std::vector<std::shared_ptr<Publisher<Cursor>>> new_partition_publishers;
  for (std::uint32_t partition = current_num_partitions;
       partition < num_partitions; ++partition) {
    new_partition_publishers.push_back(publisher_factory_(partition));
    service_composite_.AddServiceObject(new_partition_publishers.back().get());
  }
  {
    std::lock_guard<std::mutex> g{mu_};
    std::move(new_partition_publishers.begin(), new_partition_publishers.end(),
              std::back_inserter(partition_publishers_));
  }
  TryPublishMessages();
}

void MultipartitionPublisher::TriggerPublisherCreation() {
  {
    std::lock_guard<std::mutex> g{mu_};
    if (outstanding_num_partitions_req_) return;
    outstanding_num_partitions_req_.emplace();
  }
  GetNumPartitions()
      .then([this](future<StatusOr<std::uint32_t>> f) {
        if (!service_composite_.status().ok()) return;
        auto num_partitions = f.get();
        if (num_partitions.ok()) return HandleNumPartitions(*num_partitions);
        GCP_LOG(WARNING) << "Reading number of partitions for "
                         << topic_.FullName()
                         << "failed: " << num_partitions.status();
        bool first_poll;
        {
          std::lock_guard<std::mutex> g{mu_};
          first_poll = partition_publishers_.empty();
        }
        if (first_poll) {
          // fail client if first poll fails
          return service_composite_.Abort(num_partitions.status());
        }
      })
      .then([this](future<void>) {
        absl::optional<promise<void>> p;
        {
          std::lock_guard<std::mutex> g{mu_};
          assert(outstanding_num_partitions_req_);
          p.swap(outstanding_num_partitions_req_);
        }
        // only set value after done touching member variables
        p->set_value();
      });
}

void MultipartitionPublisher::RouteAndPublish(PublishState state) {
  std::uint32_t partition =
      state.message.key().empty()
          ? routing_policy_->Route(state.num_partitions)
          : routing_policy_->Route(state.message.key(), state.num_partitions);
  Publisher<Cursor>* publisher;
  {
    std::lock_guard<std::mutex> g{mu_};
    publisher = partition_publishers_.at(partition).get();
  }

  struct OnPublish {
    promise<StatusOr<MessageMetadata>> p;
    std::uint32_t partition;
    void operator()(future<StatusOr<Cursor>> f) {
      auto publish_response = f.get();
      if (!publish_response) {
        return p.set_value(std::move(publish_response).status());
      }
      p.set_value(MessageMetadata{partition, *std::move(publish_response)});
    }
  };

  publisher->Publish(std::move(state.message))
      .then(OnPublish{std::move(state.publish_promise), partition});
}

void MultipartitionPublisher::TryPublishMessages() {
  {
    std::lock_guard<std::mutex> g{mu_};
    if (in_publish_loop_) return;
    in_publish_loop_ = true;
  }
  while (true) {
    std::deque<PublishState> messages;
    uint32_t num_partitions;
    {
      std::lock_guard<std::mutex> g{mu_};
      if (messages_.empty()) {
        in_publish_loop_ = false;
        return;
      }
      messages.swap(messages_);
      num_partitions = static_cast<std::uint32_t>(partition_publishers_.size());
    }
    for (PublishState& state : messages) {
      state.num_partitions = num_partitions;
      RouteAndPublish(std::move(state));
    }
  }
}

future<StatusOr<MessageMetadata>> MultipartitionPublisher::Publish(
    PubSubMessage m) {
  PublishState state;
  state.message = std::move(m);
  future<StatusOr<MessageMetadata>> to_return;
  {
    std::lock_guard<std::mutex> g{mu_};
    to_return = state.publish_promise.get_future();
    messages_.push_back(std::move(state));
    // message will be published whenever we successfully read the number of
    // partitions and publishers are created
    if (partition_publishers_.empty()) return to_return;
  }
  TryPublishMessages();
  return to_return;
}

void MultipartitionPublisher::Flush() {
  std::vector<Publisher<Cursor>*> publishers;
  {
    std::lock_guard<std::mutex> g{mu_};
    publishers.reserve(partition_publishers_.size());
    for (auto& publisher : partition_publishers_) {
      publishers.push_back(publisher.get());
    }
  }
  for (auto* publisher : publishers) {
    publisher->Flush();
  }
}

future<void> MultipartitionPublisher::Shutdown() {
  cancel_token_ = nullptr;
  std::deque<PublishState> messages;
  {
    std::lock_guard<std::mutex> g{mu_};
    messages.swap(messages_);
  }
  for (auto& state : messages) {
    state.publish_promise.set_value(Status{
        StatusCode::kFailedPrecondition, "Multipartition publisher shutdown."});
  }

  auto shutdown = service_composite_.Shutdown();
  std::lock_guard<std::mutex> g{mu_};
  if (outstanding_num_partitions_req_) {
    return outstanding_num_partitions_req_->get_future().then(
        ChainFuture(std::move(shutdown)));
  }
  return shutdown;
}

GOOGLE_CLOUD_CPP_INLINE_NAMESPACE_END
}  // namespace pubsublite_internal
}  // namespace cloud
}  // namespace google
<commit_msg>cleanup(pubsublite): explicitly use std (#8791)<commit_after>// Copyright 2022 Google LLC
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     https://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "google/cloud/pubsublite/internal/multipartition_publisher.h"
#include <functional>
#include <limits>

namespace google {
namespace cloud {
namespace pubsublite_internal {
GOOGLE_CLOUD_CPP_INLINE_NAMESPACE_BEGIN

using ::google::cloud::pubsublite::AdminServiceConnection;
using ::google::cloud::pubsublite::MessageMetadata;
using ::google::cloud::pubsublite::Topic;
using ::google::cloud::pubsublite::v1::Cursor;
using ::google::cloud::pubsublite::v1::PubSubMessage;
using ::google::cloud::pubsublite::v1::TopicPartitions;

MultipartitionPublisher::MultipartitionPublisher(
    PartitionPublisherFactory publisher_factory,
    std::shared_ptr<AdminServiceConnection> admin_connection,
    AlarmRegistry& alarm_registry,
    std::unique_ptr<RoutingPolicy> routing_policy, Topic topic)
    : publisher_factory_{std::move(publisher_factory)},
      admin_connection_{std::move(admin_connection)},
      routing_policy_{std::move(routing_policy)},
      topic_{std::move(topic)},
      topic_partitions_request_{[&] {
        google::cloud::pubsublite::v1::GetTopicPartitionsRequest request;
        *request.mutable_name() = topic_.FullName();
        return request;
      }()},
      cancel_token_{alarm_registry.RegisterAlarm(
          std::chrono::seconds{60}, [this]() { TriggerPublisherCreation(); })} {
}

MultipartitionPublisher::~MultipartitionPublisher() {
  future<void> shutdown = Shutdown();
  if (!shutdown.is_ready()) {
    GCP_LOG(WARNING) << "`Shutdown` must be called and finished before object "
                        "goes out of scope.";
    assert(false);
  }
  shutdown.get();
}

future<Status> MultipartitionPublisher::Start() {
  auto start = service_composite_.Start();
  TriggerPublisherCreation();
  return start;
}

future<StatusOr<std::uint32_t>> MultipartitionPublisher::GetNumPartitions() {
  return admin_connection_->AsyncGetTopicPartitions(topic_partitions_request_)
      .then([](future<StatusOr<TopicPartitions>> f) -> StatusOr<std::uint32_t> {
        auto partitions = f.get();
        if (!partitions) return std::move(partitions).status();
        if (partitions->partition_count() >=
            std::numeric_limits<std::uint32_t>::max()) {
          return Status{StatusCode::kInternal,
                        absl::StrCat("Returned partition count is too big: ",
                                     partitions->partition_count())};
        }
        return static_cast<std::uint32_t>(partitions->partition_count());
      });
}

void MultipartitionPublisher::HandleNumPartitions(
    std::uint32_t num_partitions) {
  std::uint32_t current_num_partitions;
  {
    std::lock_guard<std::mutex> g{mu_};
    current_num_partitions =
        static_cast<std::uint32_t>(partition_publishers_.size());
  }
  assert(num_partitions >= current_num_partitions);  // should be no race
  if (num_partitions == current_num_partitions) return;
  std::vector<std::shared_ptr<Publisher<Cursor>>> new_partition_publishers;
  for (std::uint32_t partition = current_num_partitions;
       partition < num_partitions; ++partition) {
    new_partition_publishers.push_back(publisher_factory_(partition));
    service_composite_.AddServiceObject(new_partition_publishers.back().get());
  }
  {
    std::lock_guard<std::mutex> g{mu_};
    std::move(new_partition_publishers.begin(), new_partition_publishers.end(),
              std::back_inserter(partition_publishers_));
  }
  TryPublishMessages();
}

void MultipartitionPublisher::TriggerPublisherCreation() {
  {
    std::lock_guard<std::mutex> g{mu_};
    if (outstanding_num_partitions_req_) return;
    outstanding_num_partitions_req_.emplace();
  }
  GetNumPartitions()
      .then([this](future<StatusOr<std::uint32_t>> f) {
        if (!service_composite_.status().ok()) return;
        auto num_partitions = f.get();
        if (num_partitions.ok()) return HandleNumPartitions(*num_partitions);
        GCP_LOG(WARNING) << "Reading number of partitions for "
                         << topic_.FullName()
                         << "failed: " << num_partitions.status();
        bool first_poll;
        {
          std::lock_guard<std::mutex> g{mu_};
          first_poll = partition_publishers_.empty();
        }
        if (first_poll) {
          // fail client if first poll fails
          return service_composite_.Abort(num_partitions.status());
        }
      })
      .then([this](future<void>) {
        absl::optional<promise<void>> p;
        {
          std::lock_guard<std::mutex> g{mu_};
          assert(outstanding_num_partitions_req_);
          p.swap(outstanding_num_partitions_req_);
        }
        // only set value after done touching member variables
        p->set_value();
      });
}

void MultipartitionPublisher::RouteAndPublish(PublishState state) {
  std::uint32_t partition =
      state.message.key().empty()
          ? routing_policy_->Route(state.num_partitions)
          : routing_policy_->Route(state.message.key(), state.num_partitions);
  Publisher<Cursor>* publisher;
  {
    std::lock_guard<std::mutex> g{mu_};
    publisher = partition_publishers_.at(partition).get();
  }

  struct OnPublish {
    promise<StatusOr<MessageMetadata>> p;
    std::uint32_t partition;
    void operator()(future<StatusOr<Cursor>> f) {
      auto publish_response = f.get();
      if (!publish_response) {
        return p.set_value(std::move(publish_response).status());
      }
      p.set_value(MessageMetadata{partition, *std::move(publish_response)});
    }
  };

  publisher->Publish(std::move(state.message))
      .then(OnPublish{std::move(state.publish_promise), partition});
}

void MultipartitionPublisher::TryPublishMessages() {
  {
    std::lock_guard<std::mutex> g{mu_};
    if (in_publish_loop_) return;
    in_publish_loop_ = true;
  }
  while (true) {
    std::deque<PublishState> messages;
    std::uint32_t num_partitions;
    {
      std::lock_guard<std::mutex> g{mu_};
      if (messages_.empty()) {
        in_publish_loop_ = false;
        return;
      }
      messages.swap(messages_);
      num_partitions = static_cast<std::uint32_t>(partition_publishers_.size());
    }
    for (PublishState& state : messages) {
      state.num_partitions = num_partitions;
      RouteAndPublish(std::move(state));
    }
  }
}

future<StatusOr<MessageMetadata>> MultipartitionPublisher::Publish(
    PubSubMessage m) {
  PublishState state;
  state.message = std::move(m);
  future<StatusOr<MessageMetadata>> to_return;
  {
    std::lock_guard<std::mutex> g{mu_};
    to_return = state.publish_promise.get_future();
    messages_.push_back(std::move(state));
    // message will be published whenever we successfully read the number of
    // partitions and publishers are created
    if (partition_publishers_.empty()) return to_return;
  }
  TryPublishMessages();
  return to_return;
}

void MultipartitionPublisher::Flush() {
  std::vector<Publisher<Cursor>*> publishers;
  {
    std::lock_guard<std::mutex> g{mu_};
    publishers.reserve(partition_publishers_.size());
    for (auto& publisher : partition_publishers_) {
      publishers.push_back(publisher.get());
    }
  }
  for (auto* publisher : publishers) {
    publisher->Flush();
  }
}

future<void> MultipartitionPublisher::Shutdown() {
  cancel_token_ = nullptr;
  std::deque<PublishState> messages;
  {
    std::lock_guard<std::mutex> g{mu_};
    messages.swap(messages_);
  }
  for (auto& state : messages) {
    state.publish_promise.set_value(Status{
        StatusCode::kFailedPrecondition, "Multipartition publisher shutdown."});
  }

  auto shutdown = service_composite_.Shutdown();
  std::lock_guard<std::mutex> g{mu_};
  if (outstanding_num_partitions_req_) {
    return outstanding_num_partitions_req_->get_future().then(
        ChainFuture(std::move(shutdown)));
  }
  return shutdown;
}

GOOGLE_CLOUD_CPP_INLINE_NAMESPACE_END
}  // namespace pubsublite_internal
}  // namespace cloud
}  // namespace google
<|endoftext|>


<commit_before>
#include "../Pipeline/Matrix.h"
#include <catch.hpp>

using namespace generator;

TEST_CASE("Matrix types") {
    IdMatrix matrix(128, 128, 0, 11);
    matrix.set(33, 33, 0, 5);
    REQUIRE(matrix.get(33, 33, 0) == 5);

    for(Size x = 0; x < 128; x++) {
        for(Size y = 0; y < 128; y++) {
            matrix.set(x, y, 0, (x * y) & 0b1111);
        }
    }

    for(Size x = 0; x < 128; x++) {
        for(Size y = 0; y < 128; y++) {
            REQUIRE(matrix.get(x, y, 0) == ((x * y) & 0b1111));
        }
    }
}<commit_msg>Added TiledMatrix tests<commit_after>
#include "../Pipeline/Matrix.h"
#include <catch.hpp>

using namespace generator;

TEST_CASE("IdMatrix") {
    IdMatrix matrix(128, 128, 0, 11);
    matrix.set(33, 33, 0, 5);
    REQUIRE(matrix.get(33, 33, 0) == 5);

	SECTION("Medium matrix") {
		for(Size x = 0; x < 128; x++) {
			for(Size y = 0; y < 128; y++) {
				matrix.set(x, y, 0, (x * y) & 0b11111111111);
			}
		}

		for(Size x = 0; x < 128; x++) {
			for(Size y = 0; y < 128; y++) {
				CAPTURE(x);
				CAPTURE(y);
				REQUIRE(matrix.get(x, y, 0) == ((x * y) & 0b11111111111));
			}
		}
	}

	SECTION("Large matrix") {
		IdMatrix largeMatrix(256, 256, 0, 32);
		for(Size x = 0; x < 256; x++) {
			for(Size y = 0; y < 256; y++) {
				largeMatrix.set(x, y, 0, (x * y) & 0xffffffff);
			}
		}

		for(Size x = 0; x < 256; x++) {
			for(Size y = 0; y < 256; y++) {
				CAPTURE(x);
				CAPTURE(y);
				REQUIRE(largeMatrix.get(x, y, 0) == ((x * y) & 0xffffffff));
			}
		}
	}

	SECTION("Small matrix") {
		IdMatrix smallMatrix(64, 64, 0, 2);
		for(Size x = 0; x < 64; x++) {
			for(Size y = 0; y < 64; y++) {
				smallMatrix.set(x, y, 0, (x * y) & 0b11);
			}
		}

		for(Size x = 0; x < 64; x++) {
			for(Size y = 0; y < 64; y++) {
				CAPTURE(x);
				CAPTURE(y);
				REQUIRE(smallMatrix.get(x, y, 0) == ((x * y) & 0b11));
			}
		}
	}
}

TEST_CASE("TiledMatrix single tile") {
	TiledMatrix matrix(0, 16, 7);
	for(Int x = 0; x < 128; x++) {
		for(Int y = 0; y < 128; y++) {
			matrix.set(x, y, 0, (x * y) & 0b1111111);
		}
	}

	for(Int x = 0; x < 128; x++) {
		for(Int y = 0; y < 128; y++) {
			CAPTURE(x);
			CAPTURE(y);
			REQUIRE(matrix.get(x, y, 0) == ((x * y) & 0b1111111));
		}
	}
}

TEST_CASE("TiledMatrix multiple tiles") {
	TiledMatrix matrix(0, 16, 4);
	for(Int x = -231; x < 111; x++) {
		for(Int y = -17; y < 33; y++) {
			matrix.set(x, y, 0, (x * y) & 0xffff);
		}
	}

	for(Int x = -231; x < 111; x++) {
		for(Int y = -17; y < 33; y++) {
			CAPTURE(x);
			CAPTURE(y);
			REQUIRE(matrix.get(x, y, 0) == ((x * y) & 0xffff));
		}
	}
}

TEST_CASE("TiledMatrix OOB") {
	TiledMatrix matrix(0, 16, 4);
	for(Int x = -231; x < 111; x++) {
		for(Int y = -17; y < 33; y++) {
			matrix.set(x, y, 0, (x * y) & 0xffff);
		}
	}

	REQUIRE(matrix.get(300, 300, 0) == 0);
}<|endoftext|>