21 #include <Eigen/Geometry>    23 #include <boost/math/constants/constants.hpp>    47     static double deg2rad(
double deg);
    55     static double rad2deg(
double rad);
 static double getDistanceBetweenPoints(const Eigen::Vector3d p1, const Eigen::Vector3d p2)
static double deg2rad(double deg)
static double rad2deg(double rad)