From ead7908d7712905ddfbdfc6b5764ff2858111bb2 Mon Sep 17 00:00:00 2001 From: cwcoscar Date: Wed, 19 Apr 2023 16:19:29 +0800 Subject: [PATCH] Fix the bug of enu2Geodetic() --- geodetic_utils/include/geodetic_utils/geodetic_conv.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/geodetic_utils/include/geodetic_utils/geodetic_conv.hpp b/geodetic_utils/include/geodetic_utils/geodetic_conv.hpp index 60a5671..b7a8077 100644 --- a/geodetic_utils/include/geodetic_utils/geodetic_conv.hpp +++ b/geodetic_utils/include/geodetic_utils/geodetic_conv.hpp @@ -52,7 +52,8 @@ class GeodeticConverter // Compute ECEF to NED and NED to ECEF matrices double phiP = atan2(initial_ecef_z_, sqrt(pow(initial_ecef_x_, 2) + pow(initial_ecef_y_, 2))); - ecef_to_ned_matrix_ = nRe(phiP, initial_longitude_); + // ecef_to_ned_matrix_ = nRe(phiP, initial_longitude_); + ecef_to_ned_matrix_ = nRe(initial_latitude_, initial_longitude_); ned_to_ecef_matrix_ = nRe(initial_latitude_, initial_longitude_).transpose(); haveReference_ = true; @@ -226,4 +227,4 @@ class GeodeticConverter }; // class GeodeticConverter }; // namespace geodetic_conv -#endif // GEODETIC_CONVERTER_H_ +#endif // GEODETIC_CONVERTER_H_ \ No newline at end of file