Fix Bug in NormalSpaceSampling::findBin() (#5936)

* Fix Bug in NormalSpaceSampling::findBin()

* Avoid imprecise values for the normal[] variables

---------

Co-authored-by: Ma Ruikai <maruikai@trunk.tech>

Author: QiMingZhenFan

filters/include/pcl/filters/impl/normal_space.hpp

@@ -80,9 +80,11 @@ pcl::NormalSpaceSampling<PointT, NormalT>::isEntireBinSampled (boost::dynamic_bi
 template<typename PointT, typename NormalT> unsigned int 
 pcl::NormalSpaceSampling<PointT, NormalT>::findBin (const float *normal)
 {
-  const auto ix = static_cast<unsigned> (std::round (0.5f * (binsx_ - 1.f) * (normal[0] + 1.f)));
-  const auto iy = static_cast<unsigned> (std::round (0.5f * (binsy_ - 1.f) * (normal[1] + 1.f)));
-  const auto iz = static_cast<unsigned> (std::round (0.5f * (binsz_ - 1.f) * (normal[2] + 1.f)));
+  // in the case where normal[0] == 1.0f, ix will be binsx_, which is out of range.
+  // thus, use std::min to avoid this situation.
+  const auto ix = std::min (binsx_ - 1, static_cast<unsigned> (std::max (0.0f, std::floor (0.5f * (binsx_) * (normal[0] + 1.f)))));
+  const auto iy = std::min (binsy_ - 1, static_cast<unsigned> (std::max (0.0f, std::floor (0.5f * (binsy_) * (normal[1] + 1.f)))));
+  const auto iz = std::min (binsz_ - 1, static_cast<unsigned> (std::max (0.0f, std::floor (0.5f * (binsz_) * (normal[2] + 1.f)))));
   return ix * (binsy_*binsz_) + iy * binsz_ + iz;
 }