diff --git a/CMakeLists.txt b/CMakeLists.txt index 9096577..9a63140 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.14) +cmake_minimum_required(VERSION 3.15) project(kml_generator) find_package(ament_cmake_auto REQUIRED) diff --git a/include/kml_generator/kml_generator.hpp b/include/kml_generator/kml_generator.hpp index 89a784f..14936ec 100644 --- a/include/kml_generator/kml_generator.hpp +++ b/include/kml_generator/kml_generator.hpp @@ -5,7 +5,9 @@ #include #include #include +#include #include +#define _USE_MATH_DEFINES #include #include "kml_generator/common.hpp" diff --git a/src/kml_generator/coordinate_transformation.cpp b/src/kml_generator/coordinate_transformation.cpp index 556f5f0..5535882 100644 --- a/src/kml_generator/coordinate_transformation.cpp +++ b/src/kml_generator/coordinate_transformation.cpp @@ -1,24 +1,27 @@ #include // Ref:eagleye(BSD3 License) // https://github.com/MapIV/eagleye/blob/main-ros1/eagleye_core/coordinate/src/llh2xyz.cpp +#include +#include + void KmlGenerator::llh2xyz(double llh_pos[3], double ecef_pos[3]) { double semi_major_axis = 6378137.0000; double semi_minor_axis = 6356752.3142; - double a1 = sqrt (1-pow((semi_minor_axis/semi_major_axis), 2.0)); + double a1 = std::sqrt (1-std::pow((semi_minor_axis/semi_major_axis), 2.0)); double a2 = a1 * a1; double phi = llh_pos[0]; double lam = llh_pos[1]; double hei = llh_pos[2]; - double sin_phi = sin(phi); - double cos_phi = cos(phi); - double cos_lam = cos(lam); - double sin_lam = sin(lam); + double sin_phi = std::sin(phi); + double cos_phi = std::cos(phi); + double cos_lam = std::cos(lam); + double sin_lam = std::sin(lam); double tmp1 = 1 - a2; - double tmp2 = sqrt(1 - a2*sin_phi*sin_phi); + double tmp2 = std::sqrt(1 - a2*sin_phi*sin_phi); ecef_pos[0] = (semi_major_axis/tmp2 + hei)*cos_lam*cos_phi; ecef_pos[1] = (semi_major_axis/tmp2 + hei)*sin_lam*cos_phi; @@ -30,51 +33,51 @@ void KmlGenerator::xyz2enu(double ecef_pos[3], double ecef_base_pos[3], double e { double llh_base_pos[3]; ecef2llh(ecef_base_pos,llh_base_pos); - enu_pos[0] = ((-(sin(llh_base_pos[1])) * (ecef_pos[0] - ecef_base_pos[0])) + - ((cos(llh_base_pos[1])) * (ecef_pos[1] - ecef_base_pos[1])) + (0 * (ecef_pos[2] - ecef_base_pos[2]))); - enu_pos[1] = ((-(sin(llh_base_pos[0])) * (cos(llh_base_pos[1])) * (ecef_pos[0] - ecef_base_pos[0])) + - (-(sin(llh_base_pos[0])) * (sin(llh_base_pos[1])) * (ecef_pos[1] - ecef_base_pos[1])) + - ((cos(llh_base_pos[0])) * (ecef_pos[2] - ecef_base_pos[2]))); - enu_pos[2] = (((cos(llh_base_pos[0])) * (cos(llh_base_pos[1])) * (ecef_pos[0] - ecef_base_pos[0])) + - ((cos(llh_base_pos[0])) * (sin(llh_base_pos[1])) * (ecef_pos[1] - ecef_base_pos[1])) + - ((sin(llh_base_pos[0])) * (ecef_pos[2] - ecef_base_pos[2]))); + enu_pos[0] = ((-(std::sin(llh_base_pos[1])) * (ecef_pos[0] - ecef_base_pos[0])) + + ((std::cos(llh_base_pos[1])) * (ecef_pos[1] - ecef_base_pos[1])) + (0 * (ecef_pos[2] - ecef_base_pos[2]))); + enu_pos[1] = ((-(std::sin(llh_base_pos[0])) * (std::cos(llh_base_pos[1])) * (ecef_pos[0] - ecef_base_pos[0])) + + (-(std::sin(llh_base_pos[0])) * (std::sin(llh_base_pos[1])) * (ecef_pos[1] - ecef_base_pos[1])) + + ((std::cos(llh_base_pos[0])) * (ecef_pos[2] - ecef_base_pos[2]))); + enu_pos[2] = (((std::cos(llh_base_pos[0])) * (std::cos(llh_base_pos[1])) * (ecef_pos[0] - ecef_base_pos[0])) + + ((std::cos(llh_base_pos[0])) * (std::sin(llh_base_pos[1])) * (ecef_pos[1] - ecef_base_pos[1])) + + ((std::sin(llh_base_pos[0])) * (ecef_pos[2] - ecef_base_pos[2]))); } void KmlGenerator::ecef2llh(double ecef_pos[3], double llh_pos[3]) { double semi_major_axis = 6378137.0000; double semi_minor_axis = 6356752.3142; - double a1 = sqrt(1 - pow(semi_minor_axis / semi_major_axis, 2.0)); - double a2 = sqrt((ecef_pos[0] * ecef_pos[0]) + (ecef_pos[1] * ecef_pos[1])); + double a1 = std::sqrt(1 - std::pow(semi_minor_axis / semi_major_axis, 2.0)); + double a2 = std::sqrt((ecef_pos[0] * ecef_pos[0]) + (ecef_pos[1] * ecef_pos[1])); double a3 = 54 * (semi_minor_axis * semi_minor_axis) * (ecef_pos[2] * ecef_pos[2]); double a4 = (a2 * a2) + (1 - (a1 * a1)) * (ecef_pos[2] * ecef_pos[2]) - (a1 * a1) * (semi_major_axis * semi_major_axis - semi_minor_axis * semi_minor_axis); double a5 = ((a1 * a1) * (a1 * a1) * a3 * (a2 * a2)) / (a4 * a4 * a4); - double a6 = pow((1 + a5 + sqrt(a5 * a5 + 2 * a5)), 1.0 / 3.0); - double a7 = a3 / (3 * pow((a6 + 1 / a6 + 1), 2.0) * a4 * a4); - double a8 = sqrt(1 + 2 * (a1 * a1) * (a1 * a1) * a7); - double a9 = -(a7 * (a1 * a1) * a2) / (1 + a8) + sqrt((semi_major_axis * semi_major_axis / 2) * + double a6 = std::pow((1 + a5 + std::sqrt(a5 * a5 + 2 * a5)), 1.0 / 3.0); + double a7 = a3 / (3 * std::pow((a6 + 1 / a6 + 1), 2.0) * a4 * a4); + double a8 = std::sqrt(1 + 2 * (a1 * a1) * (a1 * a1) * a7); + double a9 = -(a7 * (a1 * a1) * a2) / (1 + a8) + std::sqrt((semi_major_axis * semi_major_axis / 2) * (1 + 1 / a8) - (a7 * (1 - (a1 * a1)) * (ecef_pos[2] * ecef_pos[2])) / (a8 * (1 + a8)) - a7 * (a2 * a2) / 2); - double a10 = sqrt((pow((a2 - (a1 * a1) * a9), 2.0)) + (ecef_pos[2] * ecef_pos[2])); - double a11 = sqrt((pow((a2 - (a1 * a1) * a9), 2.0)) + (1 - (a1 * a1)) * (ecef_pos[2] * ecef_pos[2])); + double a10 = std::sqrt((std::pow((a2 - (a1 * a1) * a9), 2.0)) + (ecef_pos[2] * ecef_pos[2])); + double a11 = std::sqrt((std::pow((a2 - (a1 * a1) * a9), 2.0)) + (1 - (a1 * a1)) * (ecef_pos[2] * ecef_pos[2])); double a12 = ((semi_minor_axis * semi_minor_axis) * ecef_pos[2]) / (semi_major_axis * a11); - llh_pos[0] = atan((ecef_pos[2] + (a1 * (semi_major_axis / semi_minor_axis)) * (a1 * (semi_major_axis / semi_minor_axis)) * a12) / a2); + llh_pos[0] = std::atan((ecef_pos[2] + (a1 * (semi_major_axis / semi_minor_axis)) * (a1 * (semi_major_axis / semi_minor_axis)) * a12) / a2); llh_pos[1] = 0; if (ecef_pos[0] >= 0) { - llh_pos[1] = (atan(ecef_pos[1] / ecef_pos[0])); + llh_pos[1] = (std::atan(ecef_pos[1] / ecef_pos[0])); } else { if (ecef_pos[0] < 0 && ecef_pos[1] >= 0) { - llh_pos[1] = M_PI + (atan(ecef_pos[1] / ecef_pos[0])); + llh_pos[1] = std::numbers::pi + (std::atan(ecef_pos[1] / ecef_pos[0])); } else { - llh_pos[1] = (atan(ecef_pos[1] / ecef_pos[0])) - M_PI; + llh_pos[1] = (std::atan(ecef_pos[1] / ecef_pos[0])) - std::numbers::pi; } } llh_pos[2] = a10 * (1 - (semi_minor_axis * semi_minor_axis) / (semi_major_axis * a11)); -} \ No newline at end of file +} diff --git a/src/kml_generator/kml_generator_core.cpp b/src/kml_generator/kml_generator_core.cpp index 32bc657..03497c0 100644 --- a/src/kml_generator/kml_generator_core.cpp +++ b/src/kml_generator/kml_generator_core.cpp @@ -1,4 +1,5 @@ #include +#include namespace kml_utils { @@ -215,7 +216,7 @@ void KmlGenerator::LLH2StringInCondition(std::string& str, double& time_last, do } else if (interval_type_ == IntervalType::DISTANCE_INTERBAL) { - double llh_pos[3] = { llh[0] * M_PI / 180, llh[1] * M_PI / 180, llh[2] * M_PI / 180 }; + double llh_pos[3] = { llh[0] * std::numbers::pi / 180, llh[1] * std::numbers::pi / 180, llh[2] * std::numbers::pi / 180 }; llh2xyz(llh_pos, ecef_pose); if (ecef_pose_last[0] == 0 && ecef_pose_last[1] == 0 && ecef_pose_last[2] == 0)