Migrate C math functions to C++ (#267)

include/mcl_3dl/chunked_kdtree.h

@@ -30,14 +30,15 @@
 #ifndef MCL_3DL_CHUNKED_KDTREE_H
 #define MCL_3DL_CHUNKED_KDTREE_H
 
-#include <pcl/point_types.h>
-#include <pcl/kdtree/kdtree.h>
-#include <pcl/kdtree/kdtree_flann.h>
-
+#include <cmath>
 #include <stdexcept>
 #include <unordered_map>
 #include <vector>
 
+#include <pcl/point_types.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/kdtree/kdtree_flann.h>
+
 namespace mcl_3dl
 {
 template <typename POINT_TYPE>
@@ -250,9 +251,9 @@ class ChunkedKdtree
   }
   ChunkId getChunkId(const POINT_TYPE& p) const
   {
-    return ChunkId(static_cast<int>(floor(p.x * pos_to_chunk_)),
-                   static_cast<int>(floor(p.y * pos_to_chunk_)),
-                   static_cast<int>(floor(p.z * pos_to_chunk_)));
+    return ChunkId(static_cast<int>(std::floor(p.x * pos_to_chunk_)),
+                   static_cast<int>(std::floor(p.y * pos_to_chunk_)),
+                   static_cast<int>(std::floor(p.z * pos_to_chunk_)));
   }
 
 protected:

include/mcl_3dl/filter.h

@@ -30,6 +30,7 @@
 #ifndef MCL_3DL_FILTER_H
 #define MCL_3DL_FILTER_H
 
+#include <cassert>
 #include <cmath>
 
 namespace mcl_3dl

include/mcl_3dl/nd.h

@@ -32,6 +32,7 @@
 
 #define _USE_MATH_DEFINES
 #include <cmath>
+
 #include <Eigen/Core>
 #include <Eigen/LU>
 
@@ -43,7 +44,7 @@ class NormalLikelihood
 public:
   explicit NormalLikelihood(const FLT_TYPE sigma)
   {
-    a_ = 1.0 / sqrtf(2.0 * M_PI * sigma * sigma);
+    a_ = 1.0 / std::sqrt(2.0 * M_PI * sigma * sigma);
     sq2_ = sigma * sigma * 2.0;
   }
   FLT_TYPE operator()(const FLT_TYPE x) const
@@ -65,12 +66,12 @@ class NormalLikelihoodNd
 
   explicit NormalLikelihoodNd(const Matrix sigma)
   {
-    a_ = 1.0 / (pow(2.0 * M_PI, 0.5 * DIMENSION) * sqrt(sigma.determinant()));
+    a_ = 1.0 / (std::pow(2.0 * M_PI, 0.5 * DIMENSION) * std::sqrt(sigma.determinant()));
     sigma_inv_ = sigma.inverse();
   }
   FLT_TYPE operator()(const Vector x) const
   {
-    return a_ * expf(-0.5 * x.transpose() * sigma_inv_ * x);
+    return a_ * std::exp(static_cast<FLT_TYPE>(-0.5 * x.transpose() * sigma_inv_ * x));
   }
 
 protected:

include/mcl_3dl/pf.h

@@ -31,6 +31,7 @@
 #define MCL_3DL_PF_H
 
 #include <algorithm>
+#include <cassert>
 #include <cmath>
 #include <functional>
 #include <random>

include/mcl_3dl/quat.h

@@ -31,6 +31,7 @@
 #define MCL_3DL_QUAT_H
 
 #include <algorithm>
+#include <cmath>
 
 #include <mcl_3dl/vec3.h>
 
@@ -61,10 +62,10 @@ class Quat
     const Vec3 yv = up_raw.cross(xv).normalized();
     const Vec3 zv = xv.cross(yv).normalized();
 
-    w_ = sqrtf(std::max(0.0, 1.0 + xv.x_ + yv.y_ + zv.z_)) / 2.0;
-    x_ = sqrtf(std::max(0.0, 1.0 + xv.x_ - yv.y_ - zv.z_)) / 2.0;
-    y_ = sqrtf(std::max(0.0, 1.0 - xv.x_ + yv.y_ - zv.z_)) / 2.0;
-    z_ = sqrtf(std::max(0.0, 1.0 - xv.x_ - yv.y_ + zv.z_)) / 2.0;
+    w_ = std::sqrt(std::max(0.0, 1.0 + xv.x_ + yv.y_ + zv.z_)) / 2.0;
+    x_ = std::sqrt(std::max(0.0, 1.0 + xv.x_ - yv.y_ - zv.z_)) / 2.0;
+    y_ = std::sqrt(std::max(0.0, 1.0 - xv.x_ + yv.y_ - zv.z_)) / 2.0;
+    z_ = std::sqrt(std::max(0.0, 1.0 - xv.x_ - yv.y_ + zv.z_)) / 2.0;
     if (zv.y_ - yv.z_ > 0)
       x_ = -x_;
     if (xv.z_ - zv.x_ > 0)
@@ -87,7 +88,7 @@ class Quat
   }
   float norm() const
   {
-    return sqrtf(dot(*this));
+    return std::sqrt(dot(*this));
   }
   bool operator==(const Quat& q) const
   {
@@ -243,7 +244,7 @@ class Quat
     if (ang > M_PI)
       ang -= 2.0 * M_PI;
     const float wsq = 1.0 - w_ * w_;
-    axis = Vec3(x_, y_, z_) / sqrtf(wsq);
+    axis = Vec3(x_, y_, z_) / std::sqrt(wsq);
   }
   void rotateAxis(const Quat& r)
   {

include/mcl_3dl/raycast.h

@@ -30,11 +30,12 @@
 #ifndef MCL_3DL_RAYCAST_H
 #define MCL_3DL_RAYCAST_H
 
+#include <cmath>
+#include <vector>
+
 #include <mcl_3dl/vec3.h>
 #include <mcl_3dl/chunked_kdtree.h>
 
-#include <vector>
-
 namespace mcl_3dl
 {
 template <typename POINT_TYPE>
@@ -75,7 +76,7 @@ class Raycast
         const float grid_min, const float grid_max)
     {
       kdtree_ = kdtree;
-      length_ = floorf((end - begin).norm() / grid_min - sqrtf(2.0));
+      length_ = std::floor((end - begin).norm() / grid_min - std::sqrt(2.0));
       inc_ = (end - begin).normalized() * grid_min;
       pos_ = begin + inc_;
       count_ = 1;
@@ -101,21 +102,21 @@ class Raycast
       std::vector<float> sqdist(1);
       if (kdtree_->radiusSearch(
               center,
-              sqrtf(2.0) * grid_max_ / 2.0, id, sqdist, 1))
+              std::sqrt(2.0) * grid_max_ / 2.0, id, sqdist, 1))
       {
         collision = true;
 
-        const float d0 = sqrtf(sqdist[0]);
+        const float d0 = std::sqrt(sqdist[0]);
         const Vec3 pos_prev = pos_ - (inc_ * 2.0);
         POINT_TYPE center_prev;
         center_prev.x = pos_prev.x_;
         center_prev.y = pos_prev.y_;
         center_prev.z = pos_prev.z_;
         if (kdtree_->radiusSearch(
                 center_prev,
-                grid_min_ * 2 + sqrtf(2.0) * grid_max_ / 2.0, id, sqdist, 1))
+                grid_min_ * 2 + std::sqrt(2.0) * grid_max_ / 2.0, id, sqdist, 1))
         {
-          const float d1 = sqrtf(sqdist[0]);
+          const float d1 = std::sqrt(sqdist[0]);
           sin_ang = fabs(d1 - d0) / (grid_min_ * 2.0);
         }
         else

include/mcl_3dl/state_6dof.h

@@ -31,6 +31,7 @@
 #define MCL_3DL_STATE_6DOF_H
 
 #include <algorithm>
+#include <cassert>
 #include <vector>
 
 #include <ros/ros.h>
@@ -76,7 +77,7 @@ class State6DOF : public mcl_3dl::pf::ParticleBase<float>
     }
   };
   RPYVec rpy_;
-  float& operator[](const size_t i) override
+  float& operator[](const size_t i)override
   {
     switch (i)
     {

include/mcl_3dl/vec3.h

@@ -30,6 +30,8 @@
 #ifndef MCL_3DL_VEC3_H
 #define MCL_3DL_VEC3_H
 
+#include <cmath>
+
 namespace mcl_3dl
 {
 class Vec3
@@ -148,7 +150,7 @@ class Vec3
   }
   float norm() const
   {
-    return sqrtf(dot(*this));
+    return std::sqrt(dot(*this));
   }
   Vec3 normalized() const
   {

include/mcl_3dl_compat/compatibility.h

@@ -30,10 +30,10 @@
 #ifndef MCL_3DL_COMPAT_COMPATIBILITY_H
 #define MCL_3DL_COMPAT_COMPATIBILITY_H
 
-#include <ros/ros.h>
-
 #include <string>
 
+#include <ros/ros.h>
+
 namespace mcl_3dl_compat
 {
 #define STATIC_ASSERT(EXPR) static_assert(EXPR, #EXPR)

include/pcl18_backports/voxel_grid.h

@@ -55,6 +55,7 @@ using VoxelGrid18 = VoxelGrid<PointT>;
 // System has old PCL; backport VoxelGrid from pcl-1.8
 
 #include <algorithm>
+#include <cmath>
 #include <functional>
 #include <limits>
 #include <string>
@@ -105,8 +106,8 @@ class VoxelGrid18 : public Filter<PointT>
     , max_b_(Eigen::Vector4i::Zero())
     , div_b_(Eigen::Vector4i::Zero())
     , divb_mul_(Eigen::Vector4i::Zero())
-    , filter_limit_min_(-FLT_MAX)
-    , filter_limit_max_(FLT_MAX)
+    , filter_limit_min_(-std::numeric_limits<float>::max())
+    , filter_limit_max_(std::numeric_limits<float>::max())
     , filter_limit_negative_(false)
     , min_points_per_voxel_(0)
   {
@@ -221,9 +222,9 @@ class VoxelGrid18 : public Filter<PointT>
   inline int
   getCentroidIndex(const PointT& p) const
   {
-    return (leaf_layout_.at((Eigen::Vector4i(static_cast<int>(floor(p.x * inverse_leaf_size_[0])),
-                                             static_cast<int>(floor(p.y * inverse_leaf_size_[1])),
-                                             static_cast<int>(floor(p.z * inverse_leaf_size_[2])), 0) -
+    return (leaf_layout_.at((Eigen::Vector4i(static_cast<int>(std::floor(p.x * inverse_leaf_size_[0])),
+                                             static_cast<int>(std::floor(p.y * inverse_leaf_size_[1])),
+                                             static_cast<int>(std::floor(p.z * inverse_leaf_size_[2])), 0) -
                              min_b_)
                                 .dot(divb_mul_)));
   }
@@ -237,9 +238,9 @@ class VoxelGrid18 : public Filter<PointT>
   inline std::vector<int>
   getNeighborCentroidIndices(const PointT& reference_point, const Eigen::MatrixXi& relative_coordinates) const
   {
-    Eigen::Vector4i ijk(static_cast<int>(floor(reference_point.x * inverse_leaf_size_[0])),
-                        static_cast<int>(floor(reference_point.y * inverse_leaf_size_[1])),
-                        static_cast<int>(floor(reference_point.z * inverse_leaf_size_[2])), 0);
+    Eigen::Vector4i ijk(static_cast<int>(std::floor(reference_point.x * inverse_leaf_size_[0])),
+                        static_cast<int>(std::floor(reference_point.y * inverse_leaf_size_[1])),
+                        static_cast<int>(std::floor(reference_point.z * inverse_leaf_size_[2])), 0);
     Eigen::Array4i diff2min = min_b_ - ijk;
     Eigen::Array4i diff2max = max_b_ - ijk;
     std::vector<int> neighbors(relative_coordinates.cols());
@@ -272,9 +273,9 @@ class VoxelGrid18 : public Filter<PointT>
   inline Eigen::Vector3i
   getGridCoordinates(float x, float y, float z) const
   {
-    return (Eigen::Vector3i(static_cast<int>(floor(x * inverse_leaf_size_[0])),
-                            static_cast<int>(floor(y * inverse_leaf_size_[1])),
-                            static_cast<int>(floor(z * inverse_leaf_size_[2]))));
+    return (Eigen::Vector3i(static_cast<int>(std::floor(x * inverse_leaf_size_[0])),
+                            static_cast<int>(std::floor(y * inverse_leaf_size_[1])),
+                            static_cast<int>(std::floor(z * inverse_leaf_size_[2]))));
   }
 
   /** \brief Returns the index in the downsampled cloud corresponding to a given set of coordinates.
@@ -303,7 +304,7 @@ class VoxelGrid18 : public Filter<PointT>
     filter_limit_max_ = limit_max;
   }
 
-  /** \brief Get the field filter limits (min/max) set by the user. The default values are -FLT_MAX, FLT_MAX.
+  /** \brief Get the field filter limits (min/max) set by the user. The default values are -std::numeric_limits<float>::max(), std::numeric_limits<float>::max().
      * \param[out] limit_min the minimum allowed field value
      * \param[out] limit_max the maximum allowed field value
      */
@@ -406,12 +407,12 @@ class VoxelGrid18 : public Filter<PointT>
     }
 
     // Compute the minimum and maximum bounding box values
-    min_b_[0] = static_cast<int>(floor(min_p[0] * inverse_leaf_size_[0]));
-    max_b_[0] = static_cast<int>(floor(max_p[0] * inverse_leaf_size_[0]));
-    min_b_[1] = static_cast<int>(floor(min_p[1] * inverse_leaf_size_[1]));
-    max_b_[1] = static_cast<int>(floor(max_p[1] * inverse_leaf_size_[1]));
-    min_b_[2] = static_cast<int>(floor(min_p[2] * inverse_leaf_size_[2]));
-    max_b_[2] = static_cast<int>(floor(max_p[2] * inverse_leaf_size_[2]));
+    min_b_[0] = static_cast<int>(std::floor(min_p[0] * inverse_leaf_size_[0]));
+    max_b_[0] = static_cast<int>(std::floor(max_p[0] * inverse_leaf_size_[0]));
+    min_b_[1] = static_cast<int>(std::floor(min_p[1] * inverse_leaf_size_[1]));
+    max_b_[1] = static_cast<int>(std::floor(max_p[1] * inverse_leaf_size_[1]));
+    min_b_[2] = static_cast<int>(std::floor(min_p[2] * inverse_leaf_size_[2]));
+    max_b_[2] = static_cast<int>(std::floor(max_p[2] * inverse_leaf_size_[2]));
 
     // Compute the number of divisions needed along all axis
     div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones();
@@ -438,11 +439,11 @@ class VoxelGrid18 : public Filter<PointT>
           continue;
 
       int ijk0 = static_cast<int>(
-          floor(input_->points[*it].x * inverse_leaf_size_[0]) - static_cast<float>(min_b_[0]));
+          std::floor(input_->points[*it].x * inverse_leaf_size_[0]) - static_cast<float>(min_b_[0]));
       int ijk1 = static_cast<int>(
-          floor(input_->points[*it].y * inverse_leaf_size_[1]) - static_cast<float>(min_b_[1]));
+          std::floor(input_->points[*it].y * inverse_leaf_size_[1]) - static_cast<float>(min_b_[1]));
       int ijk2 = static_cast<int>(
-          floor(input_->points[*it].z * inverse_leaf_size_[2]) - static_cast<float>(min_b_[2]));
+          std::floor(input_->points[*it].z * inverse_leaf_size_[2]) - static_cast<float>(min_b_[2]));
 
       // Compute the centroid leaf index
       int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2];

src/lidar_measurement_model_beam.cpp

@@ -28,6 +28,7 @@
  */
 
 #include <algorithm>
+#include <cmath>
 #include <string>
 #include <vector>
 
@@ -81,7 +82,7 @@ void LidarMeasurementModelBeam::loadConfig(
   double beam_likelihood_min;
   pnh.param("beam_likelihood", beam_likelihood_min, 0.2);
   beam_likelihood_min_ = beam_likelihood_min;
-  beam_likelihood_ = powf(beam_likelihood_min, 1.0 / static_cast<float>(num_points));
+  beam_likelihood_ = std::pow(beam_likelihood_min, 1.0 / static_cast<float>(num_points));
 
   double ang_total_ref;
   pnh.param("ang_total_ref", ang_total_ref, M_PI / 6.0);

src/lidar_measurement_model_likelihood.cpp

@@ -28,6 +28,7 @@
  */
 
 #include <algorithm>
+#include <cmath>
 #include <string>
 #include <vector>
 
@@ -143,7 +144,7 @@ LidarMeasurementResult LidarMeasurementModelLikelihood::measure(
   {
     if (kdtree->radiusSearch(p, match_dist_min_, id, sqdist, 1))
     {
-      const float dist = match_dist_min_ - std::max(sqrtf(sqdist[0]), match_dist_flat_);
+      const float dist = match_dist_min_ - std::max(std::sqrt(sqdist[0]), match_dist_flat_);
       if (dist < 0.0)
         continue;