From 17e5f468e1e08c03f4be71d867629ea6ea846f95 Mon Sep 17 00:00:00 2001 From: daifuku07 Date: Sun, 19 Nov 2017 21:14:29 +0900 Subject: [PATCH 1/6] add utm_converter lat. lng. convert to UTM --- .../util/packages/map_tools/CMakeLists.txt | 2 + .../coordinate_transformer/latlng2utm.cpp | 32 ++++ .../coordinate_transformer/utm_converter.cpp | 152 ++++++++++++++++++ .../coordinate_transformer/utm_converter.hpp | 53 ++++++ 4 files changed, 239 insertions(+) create mode 100644 ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlng2utm.cpp create mode 100644 ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.cpp create mode 100644 ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.hpp diff --git a/ros/src/util/packages/map_tools/CMakeLists.txt b/ros/src/util/packages/map_tools/CMakeLists.txt index 677c647228a..eaa2e4cc463 100644 --- a/ros/src/util/packages/map_tools/CMakeLists.txt +++ b/ros/src/util/packages/map_tools/CMakeLists.txt @@ -23,6 +23,7 @@ SET(CMAKE_CXX_FLAGS "-std=c++11 -O2 -g -Wall ${CMAKE_CXX_FLAGS}") include_directories(include ${catkin_INCLUDE_DIRS}) +add_executable(latlng2utm nodes/coordinate_transformer/latlng2utm.cpp nodes/coordinate_transformer/utm_converter.cpp) add_executable(pcd_filter nodes/pcd_filter/pcd_filter.cpp) add_executable(pcd_binarizer nodes/pcd_binarizer/pcd_binarizer.cpp) add_executable(pcd_arealist nodes/pcd_arealist/pcd_arealist.cpp) @@ -30,6 +31,7 @@ add_executable(csv2pcd nodes/pcd_converter/csv2pcd.cpp) add_executable(pcd2csv nodes/pcd_converter/pcd2csv.cpp) add_executable(map_extender nodes/map_extender/map_extender.cpp) +target_link_libraries(latlng2utm ${catkin_LIBRARIES}) target_link_libraries(pcd_filter ${catkin_LIBRARIES}) target_link_libraries(pcd_binarizer ${catkin_LIBRARIES}) target_link_libraries(pcd_arealist ${catkin_LIBRARIES}) diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlng2utm.cpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlng2utm.cpp new file mode 100644 index 00000000000..ac78166785b --- /dev/null +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlng2utm.cpp @@ -0,0 +1,32 @@ +/* + * The rights of this source code conform to + * https://github.com/CPFL/Autoware/blob/master/LICENSE + * + * (latitude, longitude) convert to UTM(x,y,zone num, zone letter) + * + */ + +#include "utm_converter.hpp" +#include +#include +#include + +int main(int argc, char **argv) { + double lat, lng; + double x, y; + int zone_num; + std::string zone_letter; + + UtmConverter converter; + + std::cout << "input latitude: "; + std::cin >> lat; + std::cout << "input longitude: "; + std::cin >> lng; + + std::tie(x, y, zone_num, zone_letter) = converter.latlng2utm(lat, lng); + std::cout << x << ", " << y << ", " << zone_num << ", " << zone_letter + << std::endl; + + return 0; +} \ No newline at end of file diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.cpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.cpp new file mode 100644 index 00000000000..fe89349ca97 --- /dev/null +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.cpp @@ -0,0 +1,152 @@ +/* + * This source code based on: + * Copyright (c) 2012-2017 Tobias Bieniek + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + */ + +#include "utm_converter.hpp" +#include +#include + + +UtmConverter::UtmConverter() + : K0_(0.9996) + , E_(0.00669438) + , E2_(E_ * E_) + , E3_(E2_ * E_) + , E_P2_(E_ / (1.0 - E_)) + + , SQRT_E_(std::sqrt(1 - E_)) + , _E((1 - SQRT_E_) / (1 + SQRT_E_)) + , _E2(_E * _E) + , _E3(_E2 * _E) + , _E4(_E3 * _E) + , _E5(_E4 * _E) + + , M1_(1 - E_ / 4 - 3 * E2_ / 64 - 5 * E3_ / 256) + , M2_(3 * E_ / 8 + 3 * E2_ / 32 + 45 * E3_ / 1024) + , M3_(15 * E2_ / 256 + 45 * E3_ / 1024), M4_(35 * E3_ / 3072) + + , R_(6378137) + + , P2_(3. / 2 * _E - 27. / 32 * _E3 + 269. / 512 * _E5) + , P3_(21. / 16 * _E2 - 55. / 32 * _E4) + , P4_(151. / 96 * _E3 - 417. / 128 * _E5) + , P5_(1097. / 512 * _E4) + +{} + +UtmConverter::~UtmConverter() = default; + +double UtmConverter::getRadian(double deg) { return deg * M_PI / 180; } + +int UtmConverter::getZoneNumber(double lat, double lng) { + if ((56 <= lat && lat < 64) && (3 <= lng && lng < 12)) + return 32; + + if ((72 <= lat && lat <= 84) && lng >= 0) { + if (lng < 9) + return 31; + else if (lng < 21) + return 33; + else if (lng < 33) + return 35; + else if (lng < 42) + return 37; + } + + return int((lng + 180) / 6) + 1; +} + +double UtmConverter::getCentralLongitude(int zone_num) { + return (zone_num - 1) * 6 - 180 + 3; +} + +std::string UtmConverter::getZoneLetter(double lat) { + const char *ZONE_LETTERS = "CDEFGHJKLMNPQRSTUVWXX"; + std::string val; + if (-80 <= lat && lat <= 84) + val = ZONE_LETTERS[int(lat + 80) >> 3]; + return val; +} + +std::tuple +UtmConverter::latlng2utm(double lat, double lng) { + int zone_num; + std::string zone_letter; + + if (-80.0 >= lat || lat >= 84.0) { + std::cout << "latitude out of range (must be between 80 deg S and 84 deg N)" + << std::endl; + } + + if (-180.0 >= lng || lng >= 180.0) { + std::cout + << "longitude out of range (must be between 180 deg W and 180 deg E)" + << std::endl; + } + + double lat_rad = UtmConverter::getRadian(lat); + double lat_sin = std::sin(lat_rad); + double lat_cos = std::cos(lat_rad); + + double lat_tan = lat_sin / lat_cos; + double lat_tan2 = lat_tan * lat_tan; + double lat_tan4 = lat_tan2 * lat_tan2; + + zone_num = UtmConverter::getZoneNumber(lat, lng); + zone_letter = UtmConverter::getZoneLetter(lat); + + double lng_rad = UtmConverter::getRadian(lng); + + double central_lng = UtmConverter::getCentralLongitude(zone_num); + double central_lng_rad = UtmConverter::getRadian(central_lng); + + double n = R_ / std::sqrt(1 - E_ * lat_sin * lat_sin); + double c = E_P2_ * lat_cos * lat_cos; + + double a = lat_cos * (lng_rad - central_lng_rad); + double a2 = a * a; + double a3 = a2 * a; + double a4 = a3 * a; + double a5 = a4 * a; + double a6 = a5 * a; + + double m = R_ * (M1_ * lat_rad - M2_ * std::sin(2 * lat_rad) + + M3_ * std::sin(4 * lat_rad) - M4_ * std::sin(6 * lat_rad)); + + double x = + K0_ * n * + (a + a3 / 6 * (1 - lat_tan2 + c) + + a5 / 120 * (5 - 18 * lat_tan2 + lat_tan4 + 72 * c - 58 * E_P2_)) + + 500000; + + double y = K0_ * (m + + n * lat_tan * + (a2 / 2 + a4 / 24 * (5 - lat_tan2 + 9 * c + 4 * c * c) + + a6 / 720 * (61 - 58 * lat_tan2 + lat_tan4 + 600 * c - + 330 * E_P2_))); + + if (lat < 0) + y += 10000000; + + return std::forward_as_tuple(x, y, zone_num, zone_letter); +} \ No newline at end of file diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.hpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.hpp new file mode 100644 index 00000000000..e1855fb3d98 --- /dev/null +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.hpp @@ -0,0 +1,53 @@ +// +// Created by gpu on 17/11/19. +// + +#ifndef UTM_CONVERTER_HPP +#define UTM_CONVERTER_HPP + + +#include +#include + +class UtmConverter +{ +public: + UtmConverter(); + ~UtmConverter(); + + std::tuple latlng2utm(double lat, double lng); + +private: + double K0_; + + double E_; + double E2_; + double E3_; + double E_P2_; + + double SQRT_E_; + double _E; + double _E2; + double _E3; + double _E4; + double _E5; + + double M1_; + double M2_; + double M3_; + double M4_; + + double R_; + + double P2_; + double P3_; + double P4_; + double P5_; + + double getRadian(double deg); + int getZoneNumber(double lat, double lng); + double getCentralLongitude(int zone_num); + std::string getZoneLetter(double lat); +}; + +#endif // UTM_CONVERTER_HPP From 577975bdeaabf4e17195086637f520e7112d188c Mon Sep 17 00:00:00 2001 From: daifuku07 Date: Sun, 20 May 2018 23:09:01 +0900 Subject: [PATCH 2/6] add PCD_converter to MGRS grid Convert point (x, y) in lat lon format to MGRS format Corresponds to Point{XYZ, XYZI, XYZRGB} --- .../util/packages/map_tools/CMakeLists.txt | 6 +- .../coordinate_transformer/latlng2utm.cpp | 32 -- .../coordinate_transformer/latlon2mgrs.cpp | 37 +++ .../coordinate_transformer/mgrs_converter.cpp | 260 ++++++++++++++++ .../coordinate_transformer/mgrs_converter.hpp | 61 ++++ .../pcd_latlon2mgrs.cpp | 281 ++++++++++++++++++ .../coordinate_transformer/utm_converter.cpp | 152 ---------- .../coordinate_transformer/utm_converter.hpp | 53 ---- 8 files changed, 643 insertions(+), 239 deletions(-) delete mode 100644 ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlng2utm.cpp create mode 100644 ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlon2mgrs.cpp create mode 100644 ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.cpp create mode 100644 ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.hpp create mode 100644 ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_latlon2mgrs.cpp delete mode 100644 ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.cpp delete mode 100644 ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.hpp diff --git a/ros/src/util/packages/map_tools/CMakeLists.txt b/ros/src/util/packages/map_tools/CMakeLists.txt index eaa2e4cc463..42079e2ca1a 100644 --- a/ros/src/util/packages/map_tools/CMakeLists.txt +++ b/ros/src/util/packages/map_tools/CMakeLists.txt @@ -23,7 +23,8 @@ SET(CMAKE_CXX_FLAGS "-std=c++11 -O2 -g -Wall ${CMAKE_CXX_FLAGS}") include_directories(include ${catkin_INCLUDE_DIRS}) -add_executable(latlng2utm nodes/coordinate_transformer/latlng2utm.cpp nodes/coordinate_transformer/utm_converter.cpp) +add_executable(latlon2mgrs nodes/coordinate_transformer/latlon2mgrs.cpp nodes/coordinate_transformer/mgrs_converter.cpp) +add_executable(pcd_latlon2mgrs nodes/coordinate_transformer/pcd_latlon2mgrs.cpp nodes/coordinate_transformer/mgrs_converter.cpp) add_executable(pcd_filter nodes/pcd_filter/pcd_filter.cpp) add_executable(pcd_binarizer nodes/pcd_binarizer/pcd_binarizer.cpp) add_executable(pcd_arealist nodes/pcd_arealist/pcd_arealist.cpp) @@ -31,7 +32,8 @@ add_executable(csv2pcd nodes/pcd_converter/csv2pcd.cpp) add_executable(pcd2csv nodes/pcd_converter/pcd2csv.cpp) add_executable(map_extender nodes/map_extender/map_extender.cpp) -target_link_libraries(latlng2utm ${catkin_LIBRARIES}) +target_link_libraries(latlon2mgrs ${catkin_LIBRARIES}) +target_link_libraries(pcd_latlon2mgrs ${catkin_LIBRARIES}) target_link_libraries(pcd_filter ${catkin_LIBRARIES}) target_link_libraries(pcd_binarizer ${catkin_LIBRARIES}) target_link_libraries(pcd_arealist ${catkin_LIBRARIES}) diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlng2utm.cpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlng2utm.cpp deleted file mode 100644 index ac78166785b..00000000000 --- a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlng2utm.cpp +++ /dev/null @@ -1,32 +0,0 @@ -/* - * The rights of this source code conform to - * https://github.com/CPFL/Autoware/blob/master/LICENSE - * - * (latitude, longitude) convert to UTM(x,y,zone num, zone letter) - * - */ - -#include "utm_converter.hpp" -#include -#include -#include - -int main(int argc, char **argv) { - double lat, lng; - double x, y; - int zone_num; - std::string zone_letter; - - UtmConverter converter; - - std::cout << "input latitude: "; - std::cin >> lat; - std::cout << "input longitude: "; - std::cin >> lng; - - std::tie(x, y, zone_num, zone_letter) = converter.latlng2utm(lat, lng); - std::cout << x << ", " << y << ", " << zone_num << ", " << zone_letter - << std::endl; - - return 0; -} \ No newline at end of file diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlon2mgrs.cpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlon2mgrs.cpp new file mode 100644 index 00000000000..a61e2fac3a6 --- /dev/null +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlon2mgrs.cpp @@ -0,0 +1,37 @@ +/* + * The rights of this source code conform to + * https://github.com/CPFL/Autoware/blob/master/LICENSE + * + * (latitude, longitude) convert to UTM(x,y,zone num, zone letter) + */ + +#include +#include +#include +#include "mgrs_converter.hpp" + +int main(int argc, char **argv) +{ + double lat, lon; + double x, y; + map_tools::MgrsConverter converter; + + std::cout << "input latitude: "; + std::cin >> lat; + std::cout << "input longitude: "; + std::cin >> lon; + + int zone_num = converter.getUtmZoneNumber(lat, lon); + std::string zone_letter = converter.getUtmZoneLetter(lat); + converter.latlon2utm(lat, lon, &x, &y); + + std::string mgrs_code; + double easting, northing; + + std::tie(mgrs_code, easting, northing) = converter.latlon2mgrs(lat, lon); + + std::cout << "UTM: " << x << ", " << y << ", " << zone_num << ", " << zone_letter << std::endl; + std::cout << "MGRS: " << mgrs_code << ", " << easting << ", " << northing << std::endl; + + return 0; +} \ No newline at end of file diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.cpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.cpp new file mode 100644 index 00000000000..0e9b433e1fb --- /dev/null +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.cpp @@ -0,0 +1,260 @@ +/* + * This UTM converter source code is based on python code: + * Copyright (c) 2012-2017 Tobias Bieniek + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include "mgrs_converter.hpp" +#include +#include +#include +#include +#include +#include "pcl/point_types.h" + +namespace map_tools +{ +MgrsConverter::MgrsConverter() + : K0_(0.9996) + , E_(0.00669438) + , E2_(E_ * E_) + , E3_(E2_ * E_) + , E_P2_(E_ / (1.0 - E_)) + , SQRT_E_(std::sqrt(1 - E_)) + , Es_((1 - SQRT_E_) / (1 + SQRT_E_)) + , Es2_(Es_ * Es_) + , Es3_(Es2_ * Es_) + , Es4_(Es3_ * Es_) + , Es5_(Es4_ * Es_) + , M1_(1 - E_ / 4 - 3 * E2_ / 64 - 5 * E3_ / 256) + , M2_(3 * E_ / 8 + 3 * E2_ / 32 + 45 * E3_ / 1024) + , M3_(15 * E2_ / 256 + 45 * E3_ / 1024) + , M4_(35 * E3_ / 3072) + , R_(6378137) + , P2_(3. / 2 * Es_ - 27. / 32 * Es3_ + 269. / 512 * Es5_) + , P3_(21. / 16 * Es2_ - 55. / 32 * Es4_) + , P4_(151. / 96 * Es3_ - 417. / 128 * Es5_) + , P5_(1097. / 512 * Es4_) +{ +} + +MgrsConverter::~MgrsConverter() = default; + +double MgrsConverter::getRadian(double deg) +{ + return deg * M_PI / 180; +} + +int MgrsConverter::getUtmZoneNumber(double lat, double lon) +{ + if ((56 <= lat && lat < 64) && (3 <= lon && lon < 12)) + return 32; + + if ((72 <= lat && lat <= 84) && lon >= 0) + { + if (lon < 9) + return 31; + else if (lon < 21) + return 33; + else if (lon < 33) + return 35; + else if (lon < 42) + return 37; + } + + return int((lon + 180) / 6) + 1; +} + +double MgrsConverter::getCentralLongitude(int zone_num) +{ + return (zone_num - 1) * 6 - 180 + 3; +} + +std::string MgrsConverter::getUtmZoneLetter(double lat) +{ + const char *ZONE_LETTERS = "CDEFGHJKLMNPQRSTUVWXX"; + std::string val; + if (-80.0 <= lat && lat <= 84.0) + val = ZONE_LETTERS[int(lat + 80) >> 3]; + return val; +} + +void MgrsConverter::latlon2utm(double lat, double lon, double *x, double *y) +{ + int zone_num; + std::string zone_letter; + + if (-80.0 >= lat || lat >= 84.0) + { + std::cout << "latitude out of range (must be between 80 deg S and 84 deg N)" << std::endl; + } + + if (-180.0 >= lon || lon >= 180.0) + { + std::cout << "longitude out of range (must be between 180 deg W and 180 deg E)" << std::endl; + } + + double lat_rad = MgrsConverter::getRadian(lat); + double lat_sin = std::sin(lat_rad); + double lat_cos = std::cos(lat_rad); + + double lat_tan = lat_sin / lat_cos; + double lat_tan2 = lat_tan * lat_tan; + double lat_tan4 = lat_tan2 * lat_tan2; + + zone_num = MgrsConverter::getUtmZoneNumber(lat, lon); + zone_letter = MgrsConverter::getUtmZoneLetter(lat); + + double lng_rad = MgrsConverter::getRadian(lon); + + double central_lng = MgrsConverter::getCentralLongitude(zone_num); + double central_lng_rad = MgrsConverter::getRadian(central_lng); + + double n = R_ / std::sqrt(1 - E_ * lat_sin * lat_sin); + double c = E_P2_ * lat_cos * lat_cos; + + double a = lat_cos * (lng_rad - central_lng_rad); + double a2 = a * a; + double a3 = a2 * a; + double a4 = a3 * a; + double a5 = a4 * a; + double a6 = a5 * a; + + double m = + R_ * (M1_ * lat_rad - M2_ * std::sin(2 * lat_rad) + M3_ * std::sin(4 * lat_rad) - M4_ * std::sin(6 * lat_rad)); + + *x = K0_ * n * (a + a3 / 6 * (1 - lat_tan2 + c) + a5 / 120 * (5 - 18 * lat_tan2 + lat_tan4 + 72 * c - 58 * E_P2_)) + + 500000; + + *y = K0_ * (m + + n * lat_tan * (a2 / 2 + a4 / 24 * (5 - lat_tan2 + 9 * c + 4 * c * c) + + a6 / 720 * (61 - 58 * lat_tan2 + lat_tan4 + 600 * c - 330 * E_P2_))); + + if (lat < 0) + *y += 10000000; +} + +std::tuple MgrsConverter::latlon2mgrs(double lat, double lon) +{ + double utm_x, utm_y; // UTM + double easting, northing; // MGRS + int zone; + + zone = getUtmZoneNumber(lat, lon); + latlon2utm(lat, lon, &utm_x, &utm_y); + easting = utm_x; + northing = utm_y; + + if (lat <= 0.0 && utm_x == 1.0e7) + { + lat = 0; + northing = 0; + } + + int ltr2low; + double pattern_offset; + int setnum = zone % 6; + + if (setnum == 1 || setnum == 4) + { + ltr2low = 0; // A + } + else if (setnum == 2 || setnum == 5) + { + ltr2low = 9; // J + } + else // if (setnum == 3 || setnum == 0) + { + ltr2low = 18; // S + } + + if (setnum % 2) + { + pattern_offset = 0.0; + } + else + { + pattern_offset = 500000.0; + } + + // calculate northing(MGRS Y in meters) + northing = std::fmod(northing, 2000000.0); + northing += pattern_offset; + if (northing >= 2000000.0) + { + northing -= 2000000.0; + } + + int letters[3]; + + // latitudeLetter + if (lat >= 72 && lat < 84.5) + { + letters[0] = 23; + } + else if (lat > -80.5 && lat < 72) + { + int idx = int(((lat + 80.0) / 8.0) + 1.0e-12); + letters[0] = lat_band[idx]; + } else{ + std::cerr << "latitude is out of scope" << std::endl; + exit(1); + } + + if (letters[0] == 21 && zone == 31 && easting == 500000.0) + { + easting = easting - 1.0; + } + + letters[1] = ltr2low + int((easting / 100000.0) - 1); + if (ltr2low == 9 && letters[1] > 13) + { + letters[1] += 1; + } + + letters[2] = int(northing / 100000.0); + if (letters[2] > 7) + { + ++letters[2]; + } + if (letters[2] > 13) + { + ++letters[2]; // if letter[2] > 13 increment twice + } + + easting = std::fmod(easting + 1e-8, 100000.0); + northing = std::fmod(northing + 1e-8, 100000.0); + + //_mgrs_code(100m square) + std::string mgrs_code; + std::ostringstream s_zone, s_eid, s_nid; + s_zone << std::setfill('0') << std::setw(2) << zone; + std::string band = std::string{ alphabet[letters[0]] }; + std::string grid_id = std::string{ alphabet[letters[1]] } + std::string{ alphabet[letters[2]] }; + + int e_id = static_cast(easting) / 100; + int n_id = static_cast(northing) / 100; + s_eid << std::setfill('0') << std::setw(3) << e_id; + s_nid << std::setfill('0') << std::setw(3) << n_id; + + mgrs_code = s_zone.str() + band + grid_id + s_eid.str() + s_nid.str(); + + return std::forward_as_tuple(mgrs_code, easting, northing); +} +} \ No newline at end of file diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.hpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.hpp new file mode 100644 index 00000000000..84e5987dafb --- /dev/null +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.hpp @@ -0,0 +1,61 @@ +/* + * The rights of this source code conform to + * https://github.com/CPFL/Autoware/blob/master/LICENSE + */ + +#ifndef MGRS_CONVERTER_HPP +#define MGRS_CONVERTER_HPP + +#include +#include +#include +#include + +namespace map_tools +{ +class MgrsConverter +{ +public: + MgrsConverter(); + ~MgrsConverter(); + + std::string getUtmZoneLetter(double lat); + int getUtmZoneNumber(double lat, double lon); + void latlon2utm(double lat, double lon, double* x, double* y); + std::tuple latlon2mgrs(double lat, double lon); + +private: + double K0_; + + double E_; + double E2_; + double E3_; + double E_P2_; + + double SQRT_E_; + double Es_; + double Es2_; + double Es3_; + double Es4_; + double Es5_; + + double M1_; + double M2_; + double M3_; + double M4_; + + double R_; + + double P2_; + double P3_; + double P4_; + double P5_; + + char alphabet[30] = "ABCDEFGHIJKLMNOPQRSTUVWXYZ"; + int lat_band[20] = {2, 3, 4, 5, 6, 7, 9, 10, 11, 12, 13, 15, 16, 17, 18, 19, 20, 21, 22, 23 }; + + double getRadian(double deg); + double getCentralLongitude(int zone_num); +}; +} +#endif // MGRS_CONVERTER_HPP diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_latlon2mgrs.cpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_latlon2mgrs.cpp new file mode 100644 index 00000000000..6b33167ada6 --- /dev/null +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_latlon2mgrs.cpp @@ -0,0 +1,281 @@ +/* + * The rights of this source code conform to + * https://github.com/CPFL/Autoware/blob/master/LICENSE + */ + +#include +#include +#include +#include + +#include "mgrs_converter.hpp" + +struct pcd_xyz_grid +{ + std::string mgrs_code; + pcl::PointCloud cloud; + unsigned int size; +}; + +struct pcd_xyzi_grid +{ + std::string mgrs_code; + pcl::PointCloud cloud; + unsigned int size; +}; + +struct pcd_xyzrgb_grid +{ + std::string mgrs_code; + pcl::PointCloud cloud; + unsigned int size; +}; + +std::string current_time() +{ + std::time_t rawtime; + std::tm *timeinfo; + char buffer[80]; + + std::time(&rawtime); + timeinfo = std::localtime(&rawtime); + + std::strftime(buffer, 80, "%Y%m%d-%H%M%S", timeinfo); + std::string date_time = buffer; + return date_time; +} + +void pcd_latlon2mgrs(const pcl::PointCloud::Ptr input, const std::string path) +{ + map_tools::MgrsConverter converter; + std::vector grids; + + for (auto &itr : *input) + { + std::string code; + double easting, northing; + std::tie(code, easting, northing) = converter.latlon2mgrs(static_cast(itr.x), static_cast(itr.y)); + itr.x = static_cast(easting); + itr.y = static_cast(northing); + const pcl::PointXYZ &p = itr; + + bool push_flag = false; + for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) + { + if (code == v_itr->mgrs_code) + { + v_itr->cloud.points.push_back(p); + ++v_itr->size; + push_flag = true; + break; + } + } + // NEW mgrs_code + pcl::PointCloud a; // null for initialize + if (!push_flag) + { + pcd_xyz_grid new_grid = { code, a, 0 }; + grids.push_back(new_grid); + grids[grids.size() - 1].cloud.points.push_back(p); + ++grids[grids.size() - 1].size; + } + } + + // save PCD to "MGRS_code_time.pcd" + for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) + { + std::string file_name = path + v_itr->mgrs_code + "_" + current_time() + ".pcd"; + v_itr->cloud.header = input->header; + v_itr->cloud.width = v_itr->size; + v_itr->cloud.height = 1; + pcl::io::savePCDFileASCII(file_name, v_itr->cloud); + std::cout << "Saved " << v_itr->cloud.points.size() << " data points to " << file_name << std::endl; + } +} + +void pcd_latlon2mgrs(pcl::PointCloud::Ptr input, std::string path) +{ + map_tools::MgrsConverter converter; + std::vector grids; + + for (auto &itr : *input) + { + std::string code; + double easting, northing; + std::tie(code, easting, northing) = converter.latlon2mgrs(static_cast(itr.x), static_cast(itr.y)); + itr.x = static_cast(easting); + itr.y = static_cast(northing); + const pcl::PointXYZI &p = itr; + + bool push_flag = false; + for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) + { + if (code == v_itr->mgrs_code) + { + v_itr->cloud.points.push_back(p); + ++v_itr->size; + push_flag = true; + break; + } + } + // NEW mgrs_code + pcl::PointCloud a; // null for initialize + if (!push_flag) + { + pcd_xyzi_grid new_grid = { code, a, 0 }; + grids.push_back(new_grid); + grids[grids.size() - 1].cloud.points.push_back(p); + ++grids[grids.size() - 1].size; + } + } + + // save PCD to "MGRS_code_time.pcd" + for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) + { + std::string file_name = path + v_itr->mgrs_code + "_" + current_time() + ".pcd"; + v_itr->cloud.header = input->header; + v_itr->cloud.width = v_itr->size; + v_itr->cloud.height = 1; + pcl::io::savePCDFileASCII(file_name, v_itr->cloud); + std::cout << "Saved " << v_itr->cloud.points.size() << " data points to " << file_name << std::endl; + } +} + +void pcd_latlon2mgrs(pcl::PointCloud::Ptr input, std::string path) +{ + map_tools::MgrsConverter converter; + std::vector grids; + + for (auto &itr : *input) + { + std::string code; + double easting, northing; + std::tie(code, easting, northing) = converter.latlon2mgrs(static_cast(itr.x), static_cast(itr.y)); + itr.x = static_cast(easting); + itr.y = static_cast(northing); + const pcl::PointXYZRGB &p = itr; + + bool push_flag = false; + for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) + { + if (code == v_itr->mgrs_code) + { + v_itr->cloud.points.push_back(p); + ++v_itr->size; + push_flag = true; + break; + } + } + // NEW mgrs_code + pcl::PointCloud a; // null for initialize + if (!push_flag) + { + pcd_xyzrgb_grid new_grid = { code, a, 0 }; + grids.push_back(new_grid); + grids[grids.size() - 1].cloud.points.push_back(p); + ++grids[grids.size() - 1].size; + } + } + + // save PCD to "MGRS_code_time.pcd" + for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) + { + std::string file_name = path + v_itr->mgrs_code + "_" + current_time() + ".pcd"; + v_itr->cloud.header = input->header; + v_itr->cloud.width = v_itr->size; + v_itr->cloud.height = 1; + pcl::io::savePCDFileASCII(file_name, v_itr->cloud); + std::cout << "Saved " << v_itr->cloud.points.size() << " data points to " << file_name << std::endl; + } +} + +int main(int argc, char **argv) +{ + if (argc < 3) + { + std::cout << "\"[PointXYZ|PointXYZI|PointXYZRGB]\" \"filename(***.pcd)\" " << std::endl; + return -1; + } + + std::string point_type = argv[1]; + if (point_type != "PointXYZ" && point_type != "PointXYZI" && point_type != "PointRGB") + { + std::cout << "\"[PointXYZ|PointXYZI|PointXYZRGB]\" \"filename(***.pcd)\" " << std::endl; + std::cout << "you have to input Point type" << std::endl; + return -1; + } + + // PointXYZ + if (point_type == "PointXYZ") + { + pcl::PointCloud map; + std::string path; + // Loading all PCDs. + for (int i = 3; i <= argc; i++) + { + std::string input = argv[i - 1]; + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + if (pcl::io::loadPCDFile(input, *temp_cloud) == -1) + { + PCL_ERROR("Couldn't read file %s \n", input.c_str()); + return -1; + }; + unsigned long path_i = input.rfind('/'); + path = input.substr(0, path_i + 1); + + map += *temp_cloud; + } + pcl::PointCloud::Ptr input(new pcl::PointCloud(map)); + pcd_latlon2mgrs(input, path); + } + + // PointXYZI + else if (point_type == "PointXYZI") + { + pcl::PointCloud map; + std::string path; + // Loading all PCDs. + for (int i = 3; i <= argc; i++) + { + std::string input = argv[i - 1]; + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + if (pcl::io::loadPCDFile(input, *temp_cloud) == -1) + { + PCL_ERROR("Couldn't read file %s \n", input.c_str()); + return -1; + }; + unsigned long path_i = input.rfind('/'); + path = input.substr(0, path_i + 1); + + map += *temp_cloud; + } + pcl::PointCloud::Ptr input(new pcl::PointCloud(map)); + pcd_latlon2mgrs(input, path); + } + + // PointRGB + else + { + pcl::PointCloud map; + std::string path; + // Loading all PCDs. + for (int i = 3; i <= argc; i++) + { + std::string input = argv[i - 1]; + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + if (pcl::io::loadPCDFile(input, *temp_cloud) == -1) + { + PCL_ERROR("Couldn't read file %s \n", input.c_str()); + return -1; + }; + unsigned long path_i = input.rfind('/'); + path = input.substr(0, path_i + 1); + + map += *temp_cloud; + } + pcl::PointCloud::Ptr input(new pcl::PointCloud(map)); + pcd_latlon2mgrs(input, path); + } + + return 0; +} diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.cpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.cpp deleted file mode 100644 index fe89349ca97..00000000000 --- a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.cpp +++ /dev/null @@ -1,152 +0,0 @@ -/* - * This source code based on: - * Copyright (c) 2012-2017 Tobias Bieniek - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - * - */ - -#include "utm_converter.hpp" -#include -#include - - -UtmConverter::UtmConverter() - : K0_(0.9996) - , E_(0.00669438) - , E2_(E_ * E_) - , E3_(E2_ * E_) - , E_P2_(E_ / (1.0 - E_)) - - , SQRT_E_(std::sqrt(1 - E_)) - , _E((1 - SQRT_E_) / (1 + SQRT_E_)) - , _E2(_E * _E) - , _E3(_E2 * _E) - , _E4(_E3 * _E) - , _E5(_E4 * _E) - - , M1_(1 - E_ / 4 - 3 * E2_ / 64 - 5 * E3_ / 256) - , M2_(3 * E_ / 8 + 3 * E2_ / 32 + 45 * E3_ / 1024) - , M3_(15 * E2_ / 256 + 45 * E3_ / 1024), M4_(35 * E3_ / 3072) - - , R_(6378137) - - , P2_(3. / 2 * _E - 27. / 32 * _E3 + 269. / 512 * _E5) - , P3_(21. / 16 * _E2 - 55. / 32 * _E4) - , P4_(151. / 96 * _E3 - 417. / 128 * _E5) - , P5_(1097. / 512 * _E4) - -{} - -UtmConverter::~UtmConverter() = default; - -double UtmConverter::getRadian(double deg) { return deg * M_PI / 180; } - -int UtmConverter::getZoneNumber(double lat, double lng) { - if ((56 <= lat && lat < 64) && (3 <= lng && lng < 12)) - return 32; - - if ((72 <= lat && lat <= 84) && lng >= 0) { - if (lng < 9) - return 31; - else if (lng < 21) - return 33; - else if (lng < 33) - return 35; - else if (lng < 42) - return 37; - } - - return int((lng + 180) / 6) + 1; -} - -double UtmConverter::getCentralLongitude(int zone_num) { - return (zone_num - 1) * 6 - 180 + 3; -} - -std::string UtmConverter::getZoneLetter(double lat) { - const char *ZONE_LETTERS = "CDEFGHJKLMNPQRSTUVWXX"; - std::string val; - if (-80 <= lat && lat <= 84) - val = ZONE_LETTERS[int(lat + 80) >> 3]; - return val; -} - -std::tuple -UtmConverter::latlng2utm(double lat, double lng) { - int zone_num; - std::string zone_letter; - - if (-80.0 >= lat || lat >= 84.0) { - std::cout << "latitude out of range (must be between 80 deg S and 84 deg N)" - << std::endl; - } - - if (-180.0 >= lng || lng >= 180.0) { - std::cout - << "longitude out of range (must be between 180 deg W and 180 deg E)" - << std::endl; - } - - double lat_rad = UtmConverter::getRadian(lat); - double lat_sin = std::sin(lat_rad); - double lat_cos = std::cos(lat_rad); - - double lat_tan = lat_sin / lat_cos; - double lat_tan2 = lat_tan * lat_tan; - double lat_tan4 = lat_tan2 * lat_tan2; - - zone_num = UtmConverter::getZoneNumber(lat, lng); - zone_letter = UtmConverter::getZoneLetter(lat); - - double lng_rad = UtmConverter::getRadian(lng); - - double central_lng = UtmConverter::getCentralLongitude(zone_num); - double central_lng_rad = UtmConverter::getRadian(central_lng); - - double n = R_ / std::sqrt(1 - E_ * lat_sin * lat_sin); - double c = E_P2_ * lat_cos * lat_cos; - - double a = lat_cos * (lng_rad - central_lng_rad); - double a2 = a * a; - double a3 = a2 * a; - double a4 = a3 * a; - double a5 = a4 * a; - double a6 = a5 * a; - - double m = R_ * (M1_ * lat_rad - M2_ * std::sin(2 * lat_rad) + - M3_ * std::sin(4 * lat_rad) - M4_ * std::sin(6 * lat_rad)); - - double x = - K0_ * n * - (a + a3 / 6 * (1 - lat_tan2 + c) + - a5 / 120 * (5 - 18 * lat_tan2 + lat_tan4 + 72 * c - 58 * E_P2_)) + - 500000; - - double y = K0_ * (m + - n * lat_tan * - (a2 / 2 + a4 / 24 * (5 - lat_tan2 + 9 * c + 4 * c * c) + - a6 / 720 * (61 - 58 * lat_tan2 + lat_tan4 + 600 * c - - 330 * E_P2_))); - - if (lat < 0) - y += 10000000; - - return std::forward_as_tuple(x, y, zone_num, zone_letter); -} \ No newline at end of file diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.hpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.hpp deleted file mode 100644 index e1855fb3d98..00000000000 --- a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/utm_converter.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// -// Created by gpu on 17/11/19. -// - -#ifndef UTM_CONVERTER_HPP -#define UTM_CONVERTER_HPP - - -#include -#include - -class UtmConverter -{ -public: - UtmConverter(); - ~UtmConverter(); - - std::tuple latlng2utm(double lat, double lng); - -private: - double K0_; - - double E_; - double E2_; - double E3_; - double E_P2_; - - double SQRT_E_; - double _E; - double _E2; - double _E3; - double _E4; - double _E5; - - double M1_; - double M2_; - double M3_; - double M4_; - - double R_; - - double P2_; - double P3_; - double P4_; - double P5_; - - double getRadian(double deg); - int getZoneNumber(double lat, double lng); - double getCentralLongitude(int zone_num); - std::string getZoneLetter(double lat); -}; - -#endif // UTM_CONVERTER_HPP From 4ccb14272a7c74be1464feaec660c2419646fdff Mon Sep 17 00:00:00 2001 From: daifuku07 Date: Wed, 30 May 2018 17:18:04 +0900 Subject: [PATCH 3/6] add converter of JP xy to lat lon corresponding to the Japanese palane cordinate system. --- .../util/packages/map_tools/CMakeLists.txt | 13 +- .../packages/map_tools/cmake/FindProj.cmake | 73 +++++ .../coordinate_transformer/coodinate_test.cpp | 69 +++++ .../coordinate_transformer/latlon2mgrs.cpp | 37 --- .../coordinate_transformer/mgrs_converter.cpp | 218 ++++++++++++- .../coordinate_transformer/mgrs_converter.hpp | 15 +- .../coordinate_transformer/pcd_jpxy2mgrs.cpp | 290 ++++++++++++++++++ 7 files changed, 651 insertions(+), 64 deletions(-) create mode 100644 ros/src/util/packages/map_tools/cmake/FindProj.cmake create mode 100644 ros/src/util/packages/map_tools/nodes/coordinate_transformer/coodinate_test.cpp delete mode 100644 ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlon2mgrs.cpp create mode 100644 ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_jpxy2mgrs.cpp diff --git a/ros/src/util/packages/map_tools/CMakeLists.txt b/ros/src/util/packages/map_tools/CMakeLists.txt index 42079e2ca1a..999c167ae3f 100644 --- a/ros/src/util/packages/map_tools/CMakeLists.txt +++ b/ros/src/util/packages/map_tools/CMakeLists.txt @@ -1,11 +1,14 @@ cmake_minimum_required(VERSION 2.8.3) project(map_tools) +SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake") + find_package(catkin REQUIRED COMPONENTS pcl_ros pcl_conversions velodyne_pointcloud ) +find_package(Proj) ################################### ## catkin specific configuration ## @@ -21,10 +24,11 @@ catkin_package( SET(CMAKE_CXX_FLAGS "-std=c++11 -O2 -g -Wall ${CMAKE_CXX_FLAGS}") -include_directories(include ${catkin_INCLUDE_DIRS}) +include_directories(include ${catkin_INCLUDE_DIRS} ${PROJ_INCLUDE_DIR}) -add_executable(latlon2mgrs nodes/coordinate_transformer/latlon2mgrs.cpp nodes/coordinate_transformer/mgrs_converter.cpp) +add_executable(coodinate_test nodes/coordinate_transformer/coodinate_test.cpp nodes/coordinate_transformer/mgrs_converter.cpp) add_executable(pcd_latlon2mgrs nodes/coordinate_transformer/pcd_latlon2mgrs.cpp nodes/coordinate_transformer/mgrs_converter.cpp) +add_executable(pcd_jpxy2mgrs nodes/coordinate_transformer/pcd_jpxy2mgrs.cpp nodes/coordinate_transformer/mgrs_converter.cpp) add_executable(pcd_filter nodes/pcd_filter/pcd_filter.cpp) add_executable(pcd_binarizer nodes/pcd_binarizer/pcd_binarizer.cpp) add_executable(pcd_arealist nodes/pcd_arealist/pcd_arealist.cpp) @@ -32,8 +36,9 @@ add_executable(csv2pcd nodes/pcd_converter/csv2pcd.cpp) add_executable(pcd2csv nodes/pcd_converter/pcd2csv.cpp) add_executable(map_extender nodes/map_extender/map_extender.cpp) -target_link_libraries(latlon2mgrs ${catkin_LIBRARIES}) -target_link_libraries(pcd_latlon2mgrs ${catkin_LIBRARIES}) +target_link_libraries(coodinate_test ${catkin_LIBRARIES} ${PROJ_LIBRARY}) +target_link_libraries(pcd_latlon2mgrs ${catkin_LIBRARIES} ${PROJ_LIBRARY}) +target_link_libraries(pcd_jpxy2mgrs ${catkin_LIBRARIES} ${PROJ_LIBRARY}) target_link_libraries(pcd_filter ${catkin_LIBRARIES}) target_link_libraries(pcd_binarizer ${catkin_LIBRARIES}) target_link_libraries(pcd_arealist ${catkin_LIBRARIES}) diff --git a/ros/src/util/packages/map_tools/cmake/FindProj.cmake b/ros/src/util/packages/map_tools/cmake/FindProj.cmake new file mode 100644 index 00000000000..063c519e5ed --- /dev/null +++ b/ros/src/util/packages/map_tools/cmake/FindProj.cmake @@ -0,0 +1,73 @@ +# Find Proj +# ~~~~~~~~~ +# Copyright (c) 2007, Martin Dobias +# Redistribution and use is allowed according to the terms of the BSD license. +# For details see the accompanying COPYING-CMAKE-SCRIPTS file. +# +# CMake module to search for Proj library +# +# If it's found it sets PROJ_FOUND to TRUE +# and following variables are set: +# PROJ_INCLUDE_DIR +# PROJ_LIBRARY + +# FIND_PATH and FIND_LIBRARY normally search standard locations +# before the specified paths. To search non-standard paths first, +# FIND_* is invoked first with specified paths and NO_DEFAULT_PATH +# and then again with no specified paths to search the default +# locations. When an earlier FIND_* succeeds, subsequent FIND_*s +# searching for the same item do nothing. + +# try to use framework on mac +# want clean framework path, not unix compatibility path +IF (APPLE) + IF (CMAKE_FIND_FRAMEWORK MATCHES "FIRST" + OR CMAKE_FRAMEWORK_PATH MATCHES "ONLY" + OR NOT CMAKE_FIND_FRAMEWORK) + SET (CMAKE_FIND_FRAMEWORK_save ${CMAKE_FIND_FRAMEWORK} CACHE STRING "" FORCE) + SET (CMAKE_FIND_FRAMEWORK "ONLY" CACHE STRING "" FORCE) + #FIND_PATH(PROJ_INCLUDE_DIR PROJ/proj_api.h) + FIND_LIBRARY(PROJ_LIBRARY PROJ) + IF (PROJ_LIBRARY) + # FIND_PATH doesn't add "Headers" for a framework + SET (PROJ_INCLUDE_DIR ${PROJ_LIBRARY}/Headers CACHE PATH "Path to a file.") + ENDIF (PROJ_LIBRARY) + SET (CMAKE_FIND_FRAMEWORK ${CMAKE_FIND_FRAMEWORK_save} CACHE STRING "" FORCE) + ENDIF () +ENDIF (APPLE) + +FIND_PATH(PROJ_INCLUDE_DIR proj_api.h + "$ENV{LIB_DIR}/include/proj" + "$ENV{LIB_DIR}/include" + #mingw + c:/msys/local/include + NO_DEFAULT_PATH + ) +FIND_PATH(PROJ_INCLUDE_DIR proj_api.h) + +FIND_LIBRARY(PROJ_LIBRARY NAMES proj PATHS + "$ENV{LIB_DIR}/lib" + #mingw + c:/msys/local/lib + NO_DEFAULT_PATH + ) +FIND_LIBRARY(PROJ_LIBRARY NAMES proj) + +IF (PROJ_INCLUDE_DIR AND PROJ_LIBRARY) + SET(PROJ_FOUND TRUE) +ENDIF (PROJ_INCLUDE_DIR AND PROJ_LIBRARY) + + +IF (PROJ_FOUND) + + IF (NOT PROJ_FIND_QUIETLY) + MESSAGE(STATUS "Found Proj: ${PROJ_LIBRARY}") + ENDIF (NOT PROJ_FIND_QUIETLY) + +ELSE (PROJ_FOUND) + + IF (PROJ_FIND_REQUIRED) + MESSAGE(FATAL_ERROR "Could not find Proj") + ENDIF (PROJ_FIND_REQUIRED) + +ENDIF (PROJ_FOUND) \ No newline at end of file diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/coodinate_test.cpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/coodinate_test.cpp new file mode 100644 index 00000000000..03e263f9dc1 --- /dev/null +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/coodinate_test.cpp @@ -0,0 +1,69 @@ +/* + * The rights of this source code conform to + * https://github.com/CPFL/Autoware/blob/master/LICENSE + * + * (latitude, longitude) convert to MGRS + * (JP x, y) convert to (latitude, longitude) or MGRS + */ + +#include +#include +#include +#include +#include +#include "mgrs_converter.hpp" + +int main(int argc, char **argv) +{ + double lat, lon, alt; + double x, y, z; + int plane, test; + map_tools::MgrsConverter converter; + + std::cout << "choose 1(JP to MGRS) or 2(lat lon to MGRS): "; + + std::cin >> test; + + if (test != 1 && test != 2) + { + std::cout << "please input 1 or 2" << std::endl; + return -1; + } + else if (test == 1) + { + std::cout << "input x: "; + std::cin >> x; + std::cout << "input y: "; + std::cin >> y; + std::cout << "input z: "; + std::cin >> z; + std::cout << "input plane: "; + std::cin >> plane; + + converter.jpxy2latlon(x, y, z, plane, lat, lon, alt); + + std::cout << "lat, lon, alt: " << std::setprecision(std::numeric_limits::max_digits10) << lat << ", " + << std::setprecision(std::numeric_limits::max_digits10) << lon << ", " << alt << std::endl; + } + else + { + std::cout << "input latitude: "; + std::cin >> lat; + std::cout << "input longitude: "; + std::cin >> lon; + } + + int zone_num = converter.getUtmZoneNumber(lat, lon); + std::string zone_letter = converter.getUtmZoneLetter(lat); + converter.latlon2utm(lat, lon, x, y); + + std::string mgrs_code; + double easting, northing; + + std::tie(mgrs_code, easting, northing) = converter.latlon2mgrs(lat, lon); + + std::cout << "UTM: " << x << ", " << y << ", " << zone_num << ", " << zone_letter << std::endl; + std::cout << "MGRS: " << mgrs_code << ", " << easting << ", " << northing << std::endl; + + return 0; +} \ No newline at end of file diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlon2mgrs.cpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlon2mgrs.cpp deleted file mode 100644 index a61e2fac3a6..00000000000 --- a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/latlon2mgrs.cpp +++ /dev/null @@ -1,37 +0,0 @@ -/* - * The rights of this source code conform to - * https://github.com/CPFL/Autoware/blob/master/LICENSE - * - * (latitude, longitude) convert to UTM(x,y,zone num, zone letter) - */ - -#include -#include -#include -#include "mgrs_converter.hpp" - -int main(int argc, char **argv) -{ - double lat, lon; - double x, y; - map_tools::MgrsConverter converter; - - std::cout << "input latitude: "; - std::cin >> lat; - std::cout << "input longitude: "; - std::cin >> lon; - - int zone_num = converter.getUtmZoneNumber(lat, lon); - std::string zone_letter = converter.getUtmZoneLetter(lat); - converter.latlon2utm(lat, lon, &x, &y); - - std::string mgrs_code; - double easting, northing; - - std::tie(mgrs_code, easting, northing) = converter.latlon2mgrs(lat, lon); - - std::cout << "UTM: " << x << ", " << y << ", " << zone_num << ", " << zone_letter << std::endl; - std::cout << "MGRS: " << mgrs_code << ", " << easting << ", " << northing << std::endl; - - return 0; -} \ No newline at end of file diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.cpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.cpp index 0e9b433e1fb..34d986ac5e7 100644 --- a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.cpp +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.cpp @@ -20,13 +20,14 @@ * SOFTWARE. */ -#include "mgrs_converter.hpp" #include +#include +#include #include #include #include #include -#include "pcl/point_types.h" +#include "mgrs_converter.hpp" namespace map_tools { @@ -56,12 +57,12 @@ MgrsConverter::MgrsConverter() MgrsConverter::~MgrsConverter() = default; -double MgrsConverter::getRadian(double deg) +double MgrsConverter::getRadian(const double& deg) { return deg * M_PI / 180; } -int MgrsConverter::getUtmZoneNumber(double lat, double lon) +int MgrsConverter::getUtmZoneNumber(const double& lat, const double& lon) { if ((56 <= lat && lat < 64) && (3 <= lon && lon < 12)) return 32; @@ -81,21 +82,170 @@ int MgrsConverter::getUtmZoneNumber(double lat, double lon) return int((lon + 180) / 6) + 1; } -double MgrsConverter::getCentralLongitude(int zone_num) +void MgrsConverter::setPlaneRef(int num, double& lat_0, double& lon_0) +{ + double lat_deg, lat_min, lon_deg, lon_min; // longitude and latitude of origin of each plane in Japan + if (num == 1) + { + lat_deg = 33; + lat_min = 0; + lon_deg = 129; + lon_min = 30; + } + else if (num == 2) + { + lat_deg = 33; + lat_min = 0; + lon_deg = 131; + lon_min = 0; + } + else if (num == 3) + { + lat_deg = 36; + lat_min = 0; + lon_deg = 132; + lon_min = 10; + } + else if (num == 4) + { + lat_deg = 33; + lat_min = 0; + lon_deg = 133; + lon_min = 30; + } + else if (num == 5) + { + lat_deg = 36; + lat_min = 0; + lon_deg = 134; + lon_min = 20; + } + else if (num == 6) + { + lat_deg = 36; + lat_min = 0; + lon_deg = 136; + lon_min = 0; + } + else if (num == 7) + { + lat_deg = 36; + lat_min = 0; + lon_deg = 137; + lon_min = 10; + } + else if (num == 8) + { + lat_deg = 36; + lat_min = 0; + lon_deg = 138; + lon_min = 30; + } + else if (num == 9) + { + lat_deg = 36; + lat_min = 0; + lon_deg = 139; + lon_min = 50; + } + else if (num == 10) + { + lat_deg = 40; + lat_min = 0; + lon_deg = 140; + lon_min = 50; + } + else if (num == 11) + { + lat_deg = 44; + lat_min = 0; + lon_deg = 140; + lon_min = 15; + } + else if (num == 12) + { + lat_deg = 44; + lat_min = 0; + lon_deg = 142; + lon_min = 15; + } + else if (num == 13) + { + lat_deg = 44; + lat_min = 0; + lon_deg = 144; + lon_min = 15; + } + else if (num == 14) + { + lat_deg = 26; + lat_min = 0; + lon_deg = 142; + lon_min = 0; + } + else if (num == 15) + { + lat_deg = 26; + lat_min = 0; + lon_deg = 127; + lon_min = 30; + } + else if (num == 16) + { + lat_deg = 26; + lat_min = 0; + lon_deg = 124; + lon_min = 0; + } + else if (num == 17) + { + lat_deg = 26; + lat_min = 0; + lon_deg = 131; + lon_min = 0; + } + else if (num == 18) + { + lat_deg = 20; + lat_min = 0; + lon_deg = 136; + lon_min = 0; + } + else if (num == 19) + { + lat_deg = 26; + lat_min = 0; + lon_deg = 154; + lon_min = 0; + } + else + { // default is plane 7 + lat_deg = 36; + lat_min = 0; + lon_deg = 137; + lon_min = 10; + } + + // longitude and latitude + lat_0 = lat_deg + lat_min / 60.0; + lon_0 = lon_deg + lon_min / 60.0; +} + +double MgrsConverter::getCentralLongitude(const int& zone_num) { return (zone_num - 1) * 6 - 180 + 3; } -std::string MgrsConverter::getUtmZoneLetter(double lat) +std::string MgrsConverter::getUtmZoneLetter(const double& lat) { - const char *ZONE_LETTERS = "CDEFGHJKLMNPQRSTUVWXX"; + const char* ZONE_LETTERS = "CDEFGHJKLMNPQRSTUVWXX"; std::string val; if (-80.0 <= lat && lat <= 84.0) val = ZONE_LETTERS[int(lat + 80) >> 3]; return val; } -void MgrsConverter::latlon2utm(double lat, double lon, double *x, double *y) +void MgrsConverter::latlon2utm(const double& lat, const double& lon, double& x, double& y) { int zone_num; std::string zone_letter; @@ -139,15 +289,15 @@ void MgrsConverter::latlon2utm(double lat, double lon, double *x, double *y) double m = R_ * (M1_ * lat_rad - M2_ * std::sin(2 * lat_rad) + M3_ * std::sin(4 * lat_rad) - M4_ * std::sin(6 * lat_rad)); - *x = K0_ * n * (a + a3 / 6 * (1 - lat_tan2 + c) + a5 / 120 * (5 - 18 * lat_tan2 + lat_tan4 + 72 * c - 58 * E_P2_)) + - 500000; + x = K0_ * n * (a + a3 / 6 * (1 - lat_tan2 + c) + a5 / 120 * (5 - 18 * lat_tan2 + lat_tan4 + 72 * c - 58 * E_P2_)) + + 500000; - *y = K0_ * (m + - n * lat_tan * (a2 / 2 + a4 / 24 * (5 - lat_tan2 + 9 * c + 4 * c * c) + - a6 / 720 * (61 - 58 * lat_tan2 + lat_tan4 + 600 * c - 330 * E_P2_))); + y = K0_ * (m + + n * lat_tan * (a2 / 2 + a4 / 24 * (5 - lat_tan2 + 9 * c + 4 * c * c) + + a6 / 720 * (61 - 58 * lat_tan2 + lat_tan4 + 600 * c - 330 * E_P2_))); if (lat < 0) - *y += 10000000; + y += 10000000; } std::tuple MgrsConverter::latlon2mgrs(double lat, double lon) @@ -157,7 +307,7 @@ std::tuple MgrsConverter::latlon2mgrs(double lat, d int zone; zone = getUtmZoneNumber(lat, lon); - latlon2utm(lat, lon, &utm_x, &utm_y); + latlon2utm(lat, lon, utm_x, utm_y); easting = utm_x; northing = utm_y; @@ -212,7 +362,9 @@ std::tuple MgrsConverter::latlon2mgrs(double lat, d { int idx = int(((lat + 80.0) / 8.0) + 1.0e-12); letters[0] = lat_band[idx]; - } else{ + } + else + { std::cerr << "latitude is out of scope" << std::endl; exit(1); } @@ -257,4 +409,36 @@ std::tuple MgrsConverter::latlon2mgrs(double lat, d return std::forward_as_tuple(mgrs_code, easting, northing); } -} \ No newline at end of file + +void MgrsConverter::jpxy2latlon(const double& x, const double& y, const double& z, const int& plane, double& lat, + double& lon, double& alt) +{ + projPJ pj_latlong, pj_utm; + double lat_0, lon_0; // reference point of Japanese plane coordinate system + setPlaneRef(plane, lat_0, lon_0); + pj_latlong = pj_init_plus("+proj=latlong"); + std::stringstream ss; + ss << "+proj=tmerc +lat_0=" << lat_0 << " +lon_0=" << lon_0 << " +k=0.9999 +x_0=0 +y_0=0 +ellps=GRS80 " + "+towgs84=0,0,0,0,0,0,0 +units=m +no_defs"; + pj_utm = pj_init_plus(ss.str().c_str()); + + double _lat = x; + double _lon = y; + double _alt = z; + + if (pj_latlong != 0 && pj_utm != 0) + { + pj_transform(pj_utm, pj_latlong, 1, 1, &_lon, &_lat, &_alt); + _lon = _lon * RAD_TO_DEG; + _lat = _lat * RAD_TO_DEG; + + lon = _lon; + lat = _lat; + alt = _alt; + } + else + { + lon = lat = alt = 0; + } +} +} diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.hpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.hpp index 84e5987dafb..77385b49a44 100644 --- a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.hpp +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.hpp @@ -19,10 +19,12 @@ class MgrsConverter MgrsConverter(); ~MgrsConverter(); - std::string getUtmZoneLetter(double lat); - int getUtmZoneNumber(double lat, double lon); - void latlon2utm(double lat, double lon, double* x, double* y); + std::string getUtmZoneLetter(const double& lat); + int getUtmZoneNumber(const double& lat, const double& lon); + void latlon2utm(const double& lat, const double& lon, double& x, double& y); std::tuple latlon2mgrs(double lat, double lon); + void jpxy2latlon(const double& x, const double& y, const double& z, const int& plane, double& lat, double& lon, + double& alt); private: double K0_; @@ -52,10 +54,11 @@ class MgrsConverter double P5_; char alphabet[30] = "ABCDEFGHIJKLMNOPQRSTUVWXYZ"; - int lat_band[20] = {2, 3, 4, 5, 6, 7, 9, 10, 11, 12, 13, 15, 16, 17, 18, 19, 20, 21, 22, 23 }; + int lat_band[20] = { 2, 3, 4, 5, 6, 7, 9, 10, 11, 12, 13, 15, 16, 17, 18, 19, 20, 21, 22, 23 }; - double getRadian(double deg); - double getCentralLongitude(int zone_num); + double getRadian(const double& deg); + double getCentralLongitude(const int& zone_num); + void setPlaneRef(int num, double &lat_0, double &lon_0); }; } #endif // MGRS_CONVERTER_HPP diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_jpxy2mgrs.cpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_jpxy2mgrs.cpp new file mode 100644 index 00000000000..8010b5baea0 --- /dev/null +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_jpxy2mgrs.cpp @@ -0,0 +1,290 @@ +/* + * The rights of this source code conform to + * https://github.com/CPFL/Autoware/blob/master/LICENSE + */ + +#include +#include +#include +#include +#include "mgrs_converter.hpp" + +struct pcd_xyz_grid +{ + std::string mgrs_code; + pcl::PointCloud cloud; + unsigned int size; +}; + +struct pcd_xyzi_grid +{ + std::string mgrs_code; + pcl::PointCloud cloud; + unsigned int size; +}; + +struct pcd_xyzrgb_grid +{ + std::string mgrs_code; + pcl::PointCloud cloud; + unsigned int size; +}; + +std::string current_time() +{ + std::time_t rawtime; + std::tm *timeinfo; + char buffer[80]; + + std::time(&rawtime); + timeinfo = std::localtime(&rawtime); + + std::strftime(buffer, 80, "%Y%m%d-%H%M%S", timeinfo); + std::string date_time = buffer; + return date_time; +} + +void pcd_jpxy2mgrs(pcl::PointCloud::Ptr input, std::string path, const int plane_zone) +{ + map_tools::MgrsConverter converter; + std::vector grids; + + for (auto &itr : *input) + { + std::string code; + double easting, northing; + double lat, lon, alt; + converter.jpxy2latlon(static_cast(itr.y), static_cast(itr.x), static_cast(itr.z), + plane_zone, lat, lon, alt); + std::tie(code, easting, northing) = converter.latlon2mgrs(lat, lon); + itr.x = static_cast(easting); + itr.y = static_cast(northing); + const pcl::PointXYZ &p = itr; + + bool push_flag = false; + for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) + { + if (code == v_itr->mgrs_code) + { + v_itr->cloud.points.push_back(p); + ++v_itr->size; + push_flag = true; + break; + } + } + // NEW mgrs_code + pcl::PointCloud a; // null for initialize + if (!push_flag) + { + pcd_xyz_grid new_grid = { code, a, 0 }; + grids.push_back(new_grid); + grids[grids.size() - 1].cloud.points.push_back(p); + ++grids[grids.size() - 1].size; + } + } + + // save PCD to "MGRS_code_time.pcd" + for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) + { + std::string file_name = path + v_itr->mgrs_code + "_" + current_time() + ".pcd"; + v_itr->cloud.header = input->header; + v_itr->cloud.width = v_itr->size; + v_itr->cloud.height = 1; + pcl::io::savePCDFileASCII(file_name, v_itr->cloud); + std::cout << "Saved " << v_itr->cloud.points.size() << " data points to " << file_name << std::endl; + } +} + +void pcd_jpxy2mgrs(pcl::PointCloud::Ptr input, std::string path, const int plane_zone) +{ + map_tools::MgrsConverter converter; + std::vector grids; + + for (auto &itr : *input) + { + std::string code; + double easting, northing; + double lat, lon, alt; + converter.jpxy2latlon(static_cast(itr.y), static_cast(itr.x), static_cast(itr.z), + plane_zone, lat, lon, alt); + std::tie(code, easting, northing) = converter.latlon2mgrs(lat, lon); + itr.x = static_cast(easting); + itr.y = static_cast(northing); + const pcl::PointXYZI &p = itr; + + bool push_flag = false; + for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) + { + if (code == v_itr->mgrs_code) + { + v_itr->cloud.points.push_back(p); + ++v_itr->size; + push_flag = true; + break; + } + } + // NEW mgrs_code + pcl::PointCloud a; // null for initialize + if (!push_flag) + { + pcd_xyzi_grid new_grid = { code, a, 0 }; + grids.push_back(new_grid); + grids[grids.size() - 1].cloud.points.push_back(p); + ++grids[grids.size() - 1].size; + } + } + + // save PCD to "MGRS_code_time.pcd" + for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) + { + std::string file_name = path + v_itr->mgrs_code + "_" + current_time() + ".pcd"; + v_itr->cloud.header = input->header; + v_itr->cloud.width = v_itr->size; + v_itr->cloud.height = 1; + pcl::io::savePCDFileASCII(file_name, v_itr->cloud); + std::cout << "Saved " << v_itr->cloud.points.size() << " data points to " << file_name << std::endl; + } +} + +void pcd_jpxy2mgrs(pcl::PointCloud::Ptr input, std::string path, const int plane_zone) +{ + map_tools::MgrsConverter converter; + std::vector grids; + + for (auto &itr : *input) + { + std::string code; + double easting, northing; + double lat, lon, alt; + converter.jpxy2latlon(static_cast(itr.y), static_cast(itr.x), static_cast(itr.z), + plane_zone, lat, lon, alt); + std::tie(code, easting, northing) = converter.latlon2mgrs(lat, lon); + itr.x = static_cast(easting); + itr.y = static_cast(northing); + const pcl::PointXYZRGB &p = itr; + + bool push_flag = false; + for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) + { + if (code == v_itr->mgrs_code) + { + v_itr->cloud.points.push_back(p); + ++v_itr->size; + push_flag = true; + break; + } + } + // NEW mgrs_code + pcl::PointCloud a; // null for initialize + if (!push_flag) + { + pcd_xyzrgb_grid new_grid = { code, a, 0 }; + grids.push_back(new_grid); + grids[grids.size() - 1].cloud.points.push_back(p); + ++grids[grids.size() - 1].size; + } + } + + // save PCD to "MGRS_code_time.pcd" + for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) + { + std::string file_name = path + v_itr->mgrs_code + "_" + current_time() + ".pcd"; + v_itr->cloud.header = input->header; + v_itr->cloud.width = v_itr->size; + v_itr->cloud.height = 1; + pcl::io::savePCDFileASCII(file_name, v_itr->cloud); + std::cout << "Saved " << v_itr->cloud.points.size() << " data points to " << file_name << std::endl; + } +} + +int main(int argc, char **argv) +{ + if (argc < 4) + { + std::cout << "\"[PointXYZ|PointXYZI|PointXYZRGB]\" \"JP plane_zone\" \"filename(***.pcd)\" " << std::endl; + return -1; + } + + std::string point_type = argv[1]; + int plane_zone = atoi(argv[2]); + if (point_type != "PointXYZ" && point_type != "PointXYZI" && point_type != "PointXYZRGB") + { + std::cout << "\"[PointXYZ|PointXYZI|PointXYZRGB]\" \"JP plane_zone\" \"filename(***.pcd)\" " << std::endl; + std::cout << "you have to input Point type" << std::endl; + return -1; + } + + // PointXYZ + if (point_type == "PointXYZ") + { + pcl::PointCloud map; + std::string path; + // Loading all PCDs. + for (int i = 4; i <= argc; i++) + { + std::string input = argv[i - 1]; + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + if (pcl::io::loadPCDFile(input, *temp_cloud) == -1) + { + PCL_ERROR("Couldn't read file %s \n", input.c_str()); + return -1; + }; + unsigned long path_i = input.rfind('/'); + path = input.substr(0, path_i + 1); + + map += *temp_cloud; + } + pcl::PointCloud::Ptr input(new pcl::PointCloud(map)); + pcd_jpxy2mgrs(input, path, plane_zone); + } + + // PointXYZI + else if (point_type == "PointXYZI") + { + pcl::PointCloud map; + std::string path; + // Loading all PCDs. + for (int i = 4; i <= argc; i++) + { + std::string input = argv[i - 1]; + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + if (pcl::io::loadPCDFile(input, *temp_cloud) == -1) + { + PCL_ERROR("Couldn't read file %s \n", input.c_str()); + return -1; + }; + unsigned long path_i = input.rfind('/'); + path = input.substr(0, path_i + 1); + + map += *temp_cloud; + } + pcl::PointCloud::Ptr input(new pcl::PointCloud(map)); + pcd_jpxy2mgrs(input, path, plane_zone); + } + + // PointRGB + else + { + pcl::PointCloud map; + std::string path; + // Loading all PCDs. + for (int i = 4; i <= argc; i++) + { + std::string input = argv[i - 1]; + pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud); + if (pcl::io::loadPCDFile(input, *temp_cloud) == -1) + { + PCL_ERROR("Couldn't read file %s \n", input.c_str()); + return -1; + }; + unsigned long path_i = input.rfind('/'); + path = input.substr(0, path_i + 1); + + map += *temp_cloud; + } + pcl::PointCloud::Ptr input(new pcl::PointCloud(map)); + pcd_jpxy2mgrs(input, path, plane_zone); + } + + return 0; +} From ccae510f4f9ca1ace1780dc62dc3aa0180e42169 Mon Sep 17 00:00:00 2001 From: daifuku07 Date: Wed, 30 May 2018 18:45:18 +0900 Subject: [PATCH 4/6] change japanese time to unix time --- .../coordinate_transformer/pcd_jpxy2mgrs.cpp | 20 +++---------------- .../pcd_latlon2mgrs.cpp | 20 +++---------------- 2 files changed, 6 insertions(+), 34 deletions(-) diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_jpxy2mgrs.cpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_jpxy2mgrs.cpp index 8010b5baea0..368bd5041a3 100644 --- a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_jpxy2mgrs.cpp +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_jpxy2mgrs.cpp @@ -30,20 +30,6 @@ struct pcd_xyzrgb_grid unsigned int size; }; -std::string current_time() -{ - std::time_t rawtime; - std::tm *timeinfo; - char buffer[80]; - - std::time(&rawtime); - timeinfo = std::localtime(&rawtime); - - std::strftime(buffer, 80, "%Y%m%d-%H%M%S", timeinfo); - std::string date_time = buffer; - return date_time; -} - void pcd_jpxy2mgrs(pcl::PointCloud::Ptr input, std::string path, const int plane_zone) { map_tools::MgrsConverter converter; @@ -86,7 +72,7 @@ void pcd_jpxy2mgrs(pcl::PointCloud::Ptr input, std::string path, // save PCD to "MGRS_code_time.pcd" for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) { - std::string file_name = path + v_itr->mgrs_code + "_" + current_time() + ".pcd"; + std::string file_name = path + v_itr->mgrs_code + "_" + std::to_string(std::time(NULL)) + ".pcd"; v_itr->cloud.header = input->header; v_itr->cloud.width = v_itr->size; v_itr->cloud.height = 1; @@ -137,7 +123,7 @@ void pcd_jpxy2mgrs(pcl::PointCloud::Ptr input, std::string path, // save PCD to "MGRS_code_time.pcd" for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) { - std::string file_name = path + v_itr->mgrs_code + "_" + current_time() + ".pcd"; + std::string file_name = path + v_itr->mgrs_code + "_" + std::to_string(std::time(NULL)) + ".pcd"; v_itr->cloud.header = input->header; v_itr->cloud.width = v_itr->size; v_itr->cloud.height = 1; @@ -188,7 +174,7 @@ void pcd_jpxy2mgrs(pcl::PointCloud::Ptr input, std::string pat // save PCD to "MGRS_code_time.pcd" for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) { - std::string file_name = path + v_itr->mgrs_code + "_" + current_time() + ".pcd"; + std::string file_name = path + v_itr->mgrs_code + "_" + std::to_string(std::time(NULL)) + ".pcd"; v_itr->cloud.header = input->header; v_itr->cloud.width = v_itr->size; v_itr->cloud.height = 1; diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_latlon2mgrs.cpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_latlon2mgrs.cpp index 6b33167ada6..df1a6fcbf1a 100644 --- a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_latlon2mgrs.cpp +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_latlon2mgrs.cpp @@ -31,20 +31,6 @@ struct pcd_xyzrgb_grid unsigned int size; }; -std::string current_time() -{ - std::time_t rawtime; - std::tm *timeinfo; - char buffer[80]; - - std::time(&rawtime); - timeinfo = std::localtime(&rawtime); - - std::strftime(buffer, 80, "%Y%m%d-%H%M%S", timeinfo); - std::string date_time = buffer; - return date_time; -} - void pcd_latlon2mgrs(const pcl::PointCloud::Ptr input, const std::string path) { map_tools::MgrsConverter converter; @@ -84,7 +70,7 @@ void pcd_latlon2mgrs(const pcl::PointCloud::Ptr input, const std: // save PCD to "MGRS_code_time.pcd" for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) { - std::string file_name = path + v_itr->mgrs_code + "_" + current_time() + ".pcd"; + std::string file_name = path + v_itr->mgrs_code + "_" + std::to_string(std::time(NULL)) + ".pcd"; v_itr->cloud.header = input->header; v_itr->cloud.width = v_itr->size; v_itr->cloud.height = 1; @@ -132,7 +118,7 @@ void pcd_latlon2mgrs(pcl::PointCloud::Ptr input, std::string pat // save PCD to "MGRS_code_time.pcd" for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) { - std::string file_name = path + v_itr->mgrs_code + "_" + current_time() + ".pcd"; + std::string file_name = path + v_itr->mgrs_code + "_" + std::to_string(std::time(NULL)) + ".pcd"; v_itr->cloud.header = input->header; v_itr->cloud.width = v_itr->size; v_itr->cloud.height = 1; @@ -180,7 +166,7 @@ void pcd_latlon2mgrs(pcl::PointCloud::Ptr input, std::string p // save PCD to "MGRS_code_time.pcd" for (auto v_itr = grids.begin(); v_itr != grids.end(); ++v_itr) { - std::string file_name = path + v_itr->mgrs_code + "_" + current_time() + ".pcd"; + std::string file_name = path + v_itr->mgrs_code + "_" + std::to_string(std::time(NULL)) + ".pcd"; v_itr->cloud.header = input->header; v_itr->cloud.width = v_itr->size; v_itr->cloud.height = 1; From f7aff116e15957974d54f9ccd326f0501419a181 Mon Sep 17 00:00:00 2001 From: daifuku07 Date: Thu, 31 May 2018 01:21:50 +0900 Subject: [PATCH 5/6] fix dependencies --- .../util/packages/map_tools/CMakeLists.txt | 10 ++++----- .../cmake/{FindProj.cmake => FindPROJ.cmake} | 22 +++++++++---------- ros/src/util/packages/map_tools/package.xml | 2 ++ 3 files changed, 18 insertions(+), 16 deletions(-) rename ros/src/util/packages/map_tools/cmake/{FindProj.cmake => FindPROJ.cmake} (80%) diff --git a/ros/src/util/packages/map_tools/CMakeLists.txt b/ros/src/util/packages/map_tools/CMakeLists.txt index 87f525da19f..8508e41a114 100644 --- a/ros/src/util/packages/map_tools/CMakeLists.txt +++ b/ros/src/util/packages/map_tools/CMakeLists.txt @@ -3,8 +3,8 @@ project(map_tools) SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake") -find_package(Proj) find_package(PCL REQUIRED) +find_package(PROJ REQUIRED) find_package(catkin REQUIRED COMPONENTS roscpp @@ -38,9 +38,9 @@ add_executable(pcd2csv nodes/pcd_converter/pcd2csv.cpp) add_executable(map_extender nodes/map_extender/map_extender.cpp) add_executable(pcd_grid_divider nodes/pcd_grid_divider/pcd_grid_divider.cpp) -target_link_libraries(coodinate_test ${catkin_LIBRARIES} ${PROJ_LIBRARY}) -target_link_libraries(pcd_latlon2mgrs ${catkin_LIBRARIES} ${PROJ_LIBRARY}) -target_link_libraries(pcd_jpxy2mgrs ${catkin_LIBRARIES} ${PROJ_LIBRARY}) +target_link_libraries(coodinate_test ${catkin_LIBRARIES} ${PROJ_LIBRARIES}) +target_link_libraries(pcd_latlon2mgrs ${catkin_LIBRARIES} ${PROJ_LIBRARIES}) +target_link_libraries(pcd_jpxy2mgrs ${catkin_LIBRARIES} ${PROJ_LIBRARIES}) target_link_libraries(pcd_filter ${catkin_LIBRARIES}) target_link_libraries(pcd_binarizer ${catkin_LIBRARIES}) target_link_libraries(pcd_arealist ${catkin_LIBRARIES}) @@ -50,7 +50,7 @@ target_link_libraries(map_extender ${catkin_LIBRARIES}) target_link_libraries(pcd_grid_divider ${catkin_LIBRARIES}) -install(TARGETS pcd_filter pcd_binarizer pcd_arealist csv2pcd pcd2csv map_extender pcd_grid_divider +install(TARGETS coodinate_test pcd_latlon2mgrs pcd_jpxy2mgrs pcd_filter pcd_binarizer pcd_arealist csv2pcd pcd2csv map_extender pcd_grid_divider ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/ros/src/util/packages/map_tools/cmake/FindProj.cmake b/ros/src/util/packages/map_tools/cmake/FindPROJ.cmake similarity index 80% rename from ros/src/util/packages/map_tools/cmake/FindProj.cmake rename to ros/src/util/packages/map_tools/cmake/FindPROJ.cmake index 063c519e5ed..3974cab4e8a 100644 --- a/ros/src/util/packages/map_tools/cmake/FindProj.cmake +++ b/ros/src/util/packages/map_tools/cmake/FindPROJ.cmake @@ -9,7 +9,7 @@ # If it's found it sets PROJ_FOUND to TRUE # and following variables are set: # PROJ_INCLUDE_DIR -# PROJ_LIBRARY +# PROJ_LIBRARIES # FIND_PATH and FIND_LIBRARY normally search standard locations # before the specified paths. To search non-standard paths first, @@ -27,11 +27,11 @@ IF (APPLE) SET (CMAKE_FIND_FRAMEWORK_save ${CMAKE_FIND_FRAMEWORK} CACHE STRING "" FORCE) SET (CMAKE_FIND_FRAMEWORK "ONLY" CACHE STRING "" FORCE) #FIND_PATH(PROJ_INCLUDE_DIR PROJ/proj_api.h) - FIND_LIBRARY(PROJ_LIBRARY PROJ) - IF (PROJ_LIBRARY) + FIND_LIBRARY(PROJ_LIBRARIES PROJ) + IF (PROJ_LIBRARIES) # FIND_PATH doesn't add "Headers" for a framework - SET (PROJ_INCLUDE_DIR ${PROJ_LIBRARY}/Headers CACHE PATH "Path to a file.") - ENDIF (PROJ_LIBRARY) + SET (PROJ_INCLUDE_DIR ${PROJ_LIBRARIES}/Headers CACHE PATH "Path to a file.") + ENDIF (PROJ_LIBRARIES) SET (CMAKE_FIND_FRAMEWORK ${CMAKE_FIND_FRAMEWORK_save} CACHE STRING "" FORCE) ENDIF () ENDIF (APPLE) @@ -45,23 +45,23 @@ FIND_PATH(PROJ_INCLUDE_DIR proj_api.h ) FIND_PATH(PROJ_INCLUDE_DIR proj_api.h) -FIND_LIBRARY(PROJ_LIBRARY NAMES proj PATHS +FIND_LIBRARY(PROJ_LIBRARIES NAMES proj PATHS "$ENV{LIB_DIR}/lib" #mingw c:/msys/local/lib NO_DEFAULT_PATH ) -FIND_LIBRARY(PROJ_LIBRARY NAMES proj) +FIND_LIBRARY(PROJ_LIBRARIES NAMES proj) -IF (PROJ_INCLUDE_DIR AND PROJ_LIBRARY) +IF (PROJ_INCLUDE_DIR AND PROJ_LIBRARIES) SET(PROJ_FOUND TRUE) -ENDIF (PROJ_INCLUDE_DIR AND PROJ_LIBRARY) +ENDIF (PROJ_INCLUDE_DIR AND PROJ_LIBRARIES) IF (PROJ_FOUND) IF (NOT PROJ_FIND_QUIETLY) - MESSAGE(STATUS "Found Proj: ${PROJ_LIBRARY}") + MESSAGE(STATUS "Found Proj: ${PROJ_LIBRARIES}") ENDIF (NOT PROJ_FIND_QUIETLY) ELSE (PROJ_FOUND) @@ -70,4 +70,4 @@ ELSE (PROJ_FOUND) MESSAGE(FATAL_ERROR "Could not find Proj") ENDIF (PROJ_FIND_REQUIRED) -ENDIF (PROJ_FOUND) \ No newline at end of file +ENDIF (PROJ_FOUND) diff --git a/ros/src/util/packages/map_tools/package.xml b/ros/src/util/packages/map_tools/package.xml index 0ce433e0961..765c2bf6b63 100644 --- a/ros/src/util/packages/map_tools/package.xml +++ b/ros/src/util/packages/map_tools/package.xml @@ -13,6 +13,7 @@ pcl_ros pcl_conversions libpcl-all-dev + proj roscpp tf @@ -20,5 +21,6 @@ pcl_ros pcl_conversions libpcl-all-dev + proj From 53218bcd81deb6692d834dc04cffa6cae7798262 Mon Sep 17 00:00:00 2001 From: daifuku07 Date: Thu, 31 May 2018 14:19:10 +0900 Subject: [PATCH 6/6] fix compile error in gcc 4.9 --- .../map_tools/nodes/coordinate_transformer/mgrs_converter.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.hpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.hpp index 77385b49a44..40414724f41 100644 --- a/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.hpp +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.hpp @@ -53,7 +53,7 @@ class MgrsConverter double P4_; double P5_; - char alphabet[30] = "ABCDEFGHIJKLMNOPQRSTUVWXYZ"; + std::string alphabet = "ABCDEFGHIJKLMNOPQRSTUVWXYZ"; int lat_band[20] = { 2, 3, 4, 5, 6, 7, 9, 10, 11, 12, 13, 15, 16, 17, 18, 19, 20, 21, 22, 23 }; double getRadian(const double& deg);