<commit_before>/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, <NAME>, 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 <NAME>, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <cmath>

#include <boost/noncopyable.hpp>
#include <boost/make_shared.hpp>

#include <Eigen/Dense>

namespace pcl
{
  namespace ndt
  {

    template<unsigned N=3, typename T=double>
    struct ValueAndDerivatives
    {
      Eigen::Matrix<T, N, N> hessian;
      Eigen::Matrix<T, N, 1>    grad;
      T value;
      
      static ValueAndDerivatives<N,T>
      Zero ()
      {
        ValueAndDerivatives<N,T> r;
        r.hessian = Eigen::Matrix<T, N, N>::Zero ();
        r.grad    = Eigen::Matrix<T, N, 1>::Zero ();
        r.value   = 0;
        return r;
      }

      ValueAndDerivatives<N,T>&
      operator+= (ValueAndDerivatives<N,T> const& r)
      {
        hessian += r.hessian;
        grad    += r.grad;
        value   += r.value;
        return *this;
      }
    };

    template <typename PointT>
    class NormalDist
    {
        typedef pcl::PointCloud<PointT> PointCloud;
      public:
        NormalDist ()
          : min_n_ (3), n_ (0), pt_indices_ (), mean_ (), covar_inv_ ()
        {
        }
        
        /** \brief Store a point index to use later for estimating distribution parameters.
          * \param i Point index to store
          */
        void
        addIdx (size_t i)
        {
          pt_indices_.push_back (i);
        }
        
        /** \brief Estimate the normal distribution parameters given the point indices provided. Memory of point indices is cleared.
          * \param cloud Point cloud corresponding to indices passed to addIdx
          * \param min_covar_eigvalue_mult set the smallest eigenvalue to this times the largest
          */
        void
        estimateParams (const PointCloud& cloud, double min_covar_eigvalue_mult=0.001)
        {
          Eigen::Vector2d sx  = Eigen::Vector2d::Zero ();
          Eigen::Matrix2d sxx = Eigen::Matrix2d::Zero ();
          
          std::vector<size_t>::const_iterator i;
          for (i = pt_indices_.begin (); i != pt_indices_.end (); i++)
          {
            Eigen::Vector2d p (cloud[*i]. x, cloud[*i]. y);
            sx  += p;
            sxx += p * p.transpose ();
          }
          
          n_ = pt_indices_.size ();

          if (n_ >= min_n_)
          {
            mean_ = sx / n_;
            // Using maximum likelihood estimation as in the original paper
            Eigen::Matrix2d covar = (sxx - 2 * (sx * mean_.transpose ())) / n_ + mean_ * mean_.transpose ();

            Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solver (covar);
            if (solver.eigenvalues ()[0] < min_covar_eigvalue_mult * solver.eigenvalues ()[1])
            {
              PCL_DEBUG ("NDT normal fit: adjusting eigenvalue %f\n", solver.eigenvalues ()[0]);
              Eigen::Matrix2d l = solver.eigenvalues ().asDiagonal ();
              Eigen::Matrix2d q = solver.eigenvectors ();
              // set minimum smallest eigenvalue:
              l (0,0) = l (1,1) * min_covar_eigvalue_mult;
              covar = q * l * q.transpose ();
            }
            covar_inv_ = covar.inverse ();
          }

          pt_indices_.clear ();
        }

        /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution.
          *
          * estimateParams must have been called after at least three points were provided, or this will return zero.
          *
          */
        ValueAndDerivatives<3,double>
        test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
        {
          if (n_ < min_n_)
            return ValueAndDerivatives<3,double>::Zero ();
          
          ValueAndDerivatives<3,double> r;
          const double x = transformed_pt.x;
          const double y = transformed_pt.y;
          const Eigen::Vector2d p_xy (transformed_pt.x, transformed_pt.y);
          const Eigen::Vector2d q = p_xy - mean_;
          const Eigen::RowVector2d qt_cvi = q.transpose () * covar_inv_;
          const double exp_qt_cvi_q = std::exp (-0.5 * double (qt_cvi * q));
          r.value = -exp_qt_cvi_q;

          Eigen::Matrix<double, 2, 3> jacobian;
          jacobian <<
            1, 0, -(x * sin_theta + y*cos_theta),
            0, 1,   x * cos_theta - y*sin_theta;
          
          for (size_t i = 0; i < 3; i++)
            r.grad[i] = double (qt_cvi * jacobian.col (i)) * exp_qt_cvi_q;
          
          // second derivative only for i == j == 2:
          const Eigen::Vector2d d2q_didj (
              y * sin_theta - x*cos_theta,
            -(x * sin_theta + y*cos_theta)
          );

          for (size_t i = 0; i < 3; i++)
            for (size_t j = 0; j < 3; j++)
              r.hessian (i,j) = -exp_qt_cvi_q * (
                double (-qt_cvi*jacobian.col (i)) * double (-qt_cvi*jacobian.col (j)) +
                (-qt_cvi * ((i==2 && j==2)? d2q_didj : Eigen::Vector2d::Zero ())) +
                (-jacobian.col (j).transpose () * covar_inv_ * jacobian.col (i))
              );
          
          return r;
        }

    protected:
        const size_t min_n_;

        size_t n_;
        std::vector<size_t> pt_indices_;
        Eigen::Vector2d mean_;
        Eigen::Matrix2d covar_inv_;
    };

    template <typename PointT> 
    class NDTSingleGrid: public boost::noncopyable
    {
        typedef typename pcl::PointCloud<PointT> PointCloud;
        typedef typename pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr;
        typedef NormalDist<PointT> NormalDist;<commit_msg>fixed compiler issue on GCC 4.6<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, <NAME>, 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 <NAME>, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include <cmath>

#include <boost/noncopyable.hpp>
#include <boost/make_shared.hpp>

#include <Eigen/Dense>

namespace pcl
{
  namespace ndt
  {

    template<unsigned N=3, typename T=double>
    struct ValueAndDerivatives
    {
      Eigen::Matrix<T, N, N> hessian;
      Eigen::Matrix<T, N, 1>    grad;
      T value;
      
      static ValueAndDerivatives<N,T>
      Zero ()
      {
        ValueAndDerivatives<N,T> r;
        r.hessian = Eigen::Matrix<T, N, N>::Zero ();
        r.grad    = Eigen::Matrix<T, N, 1>::Zero ();
        r.value   = 0;
        return r;
      }

      ValueAndDerivatives<N,T>&
      operator+= (ValueAndDerivatives<N,T> const& r)
      {
        hessian += r.hessian;
        grad    += r.grad;
        value   += r.value;
        return *this;
      }
    };

    template <typename PointT>
    class NormalDist
    {
        typedef pcl::PointCloud<PointT> PointCloud;
      public:
        NormalDist ()
          : min_n_ (3), n_ (0), pt_indices_ (), mean_ (), covar_inv_ ()
        {
        }
        
        /** \brief Store a point index to use later for estimating distribution parameters.
          * \param i Point index to store
          */
        void
        addIdx (size_t i)
        {
          pt_indices_.push_back (i);
        }
        
        /** \brief Estimate the normal distribution parameters given the point indices provided. Memory of point indices is cleared.
          * \param cloud Point cloud corresponding to indices passed to addIdx
          * \param min_covar_eigvalue_mult set the smallest eigenvalue to this times the largest
          */
        void
        estimateParams (const PointCloud& cloud, double min_covar_eigvalue_mult=0.001)
        {
          Eigen::Vector2d sx  = Eigen::Vector2d::Zero ();
          Eigen::Matrix2d sxx = Eigen::Matrix2d::Zero ();
          
          std::vector<size_t>::const_iterator i;
          for (i = pt_indices_.begin (); i != pt_indices_.end (); i++)
          {
            Eigen::Vector2d p (cloud[*i]. x, cloud[*i]. y);
            sx  += p;
            sxx += p * p.transpose ();
          }
          
          n_ = pt_indices_.size ();

          if (n_ >= min_n_)
          {
            mean_ = sx / n_;
            // Using maximum likelihood estimation as in the original paper
            Eigen::Matrix2d covar = (sxx - 2 * (sx * mean_.transpose ())) / n_ + mean_ * mean_.transpose ();

            Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solver (covar);
            if (solver.eigenvalues ()[0] < min_covar_eigvalue_mult * solver.eigenvalues ()[1])
            {
              PCL_DEBUG ("NDT normal fit: adjusting eigenvalue %f\n", solver.eigenvalues ()[0]);
              Eigen::Matrix2d l = solver.eigenvalues ().asDiagonal ();
              Eigen::Matrix2d q = solver.eigenvectors ();
              // set minimum smallest eigenvalue:
              l (0,0) = l (1,1) * min_covar_eigvalue_mult;
              covar = q * l * q.transpose ();
            }
            covar_inv_ = covar.inverse ();
          }

          pt_indices_.clear ();
        }

        /** \brief Return the 'score' (denormalised likelihood) and derivatives of score of the point p given this distribution.
          *
          * estimateParams must have been called after at least three points were provided, or this will return zero.
          *
          */
        ValueAndDerivatives<3,double>
        test (const PointT& transformed_pt, const double& cos_theta, const double& sin_theta) const
        {
          if (n_ < min_n_)
            return ValueAndDerivatives<3,double>::Zero ();
          
          ValueAndDerivatives<3,double> r;
          const double x = transformed_pt.x;
          const double y = transformed_pt.y;
          const Eigen::Vector2d p_xy (transformed_pt.x, transformed_pt.y);
          const Eigen::Vector2d q = p_xy - mean_;
          const Eigen::RowVector2d qt_cvi = q.transpose () * covar_inv_;
          const double exp_qt_cvi_q = std::exp (-0.5 * double (qt_cvi * q));
          r.value = -exp_qt_cvi_q;

          Eigen::Matrix<double, 2, 3> jacobian;
          jacobian <<
            1, 0, -(x * sin_theta + y*cos_theta),
            0, 1,   x * cos_theta - y*sin_theta;
          
          for (size_t i = 0; i < 3; i++)
            r.grad[i] = double (qt_cvi * jacobian.col (i)) * exp_qt_cvi_q;
          
          // second derivative only for i == j == 2:
          const Eigen::Vector2d d2q_didj (
              y * sin_theta - x*cos_theta,
            -(x * sin_theta + y*cos_theta)
          );

          for (size_t i = 0; i < 3; i++)
            for (size_t j = 0; j < 3; j++)
              r.hessian (i,j) = -exp_qt_cvi_q * (
                double (-qt_cvi*jacobian.col (i)) * double (-qt_cvi*jacobian.col (j)) +
                (-qt_cvi * ((i==2 && j==2)? d2q_didj : Eigen::Vector2d::Zero ())) +
                (-jacobian.col (j).transpose () * covar_inv_ * jacobian.col (i))
              );
          
          return r;
        }

    protected:
        const size_t min_n_;

        size_t n_;
        std::vector<size_t> pt_indices_;
        Eigen::Vector2d mean_;
        Eigen::Matrix2d covar_inv_;
    };

    template <typename PointT> 
    class NDTSingleGrid: public boost::noncopyable
    {
        typedef typename pcl::PointCloud<PointT> PointCloud;
        typedef typename pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr;
        typedef typename pcl::ndt::NormalDist<PointT> NormalDist;
{'fraction_non_alphanumeric': 0.10119089073055226, 'fraction_numerical': 0.008496413399261786, 'mean_word_length': 2.0948275862068964, 'pattern_counts': {'":': 0, '<': 73, '<?xml version=': 0, '>': 61, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5389772', 'n_tokens_mistral': 4470, 'n_tokens_neox': 4224, 'n_words': 1319}