diff --git a/ros/src/util/packages/map_tools/CMakeLists.txt b/ros/src/util/packages/map_tools/CMakeLists.txt index 92ac73a0fbc..8508e41a114 100644 --- a/ros/src/util/packages/map_tools/CMakeLists.txt +++ b/ros/src/util/packages/map_tools/CMakeLists.txt @@ -1,7 +1,10 @@ cmake_minimum_required(VERSION 2.8.3) project(map_tools) +SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake") + find_package(PCL REQUIRED) +find_package(PROJ REQUIRED) find_package(catkin REQUIRED COMPONENTS roscpp @@ -22,8 +25,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(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,6 +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_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}) @@ -41,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}) \ No newline at end of file + 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 new file mode 100644 index 00000000000..3974cab4e8a --- /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_LIBRARIES + +# 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_LIBRARIES PROJ) + IF (PROJ_LIBRARIES) + # FIND_PATH doesn't add "Headers" for a framework + 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) + +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_LIBRARIES NAMES proj PATHS + "$ENV{LIB_DIR}/lib" + #mingw + c:/msys/local/lib + NO_DEFAULT_PATH + ) +FIND_LIBRARY(PROJ_LIBRARIES NAMES proj) + +IF (PROJ_INCLUDE_DIR AND PROJ_LIBRARIES) + SET(PROJ_FOUND TRUE) +ENDIF (PROJ_INCLUDE_DIR AND PROJ_LIBRARIES) + + +IF (PROJ_FOUND) + + IF (NOT PROJ_FIND_QUIETLY) + MESSAGE(STATUS "Found Proj: ${PROJ_LIBRARIES}") + 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) 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/mgrs_converter.cpp b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.cpp new file mode 100644 index 00000000000..34d986ac5e7 --- /dev/null +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.cpp @@ -0,0 +1,444 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include "mgrs_converter.hpp" + +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(const double& deg) +{ + return deg * M_PI / 180; +} + +int MgrsConverter::getUtmZoneNumber(const double& lat, const 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; +} + +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(const 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(const double& lat, const 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); +} + +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 new file mode 100644 index 00000000000..40414724f41 --- /dev/null +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/mgrs_converter.hpp @@ -0,0 +1,64 @@ +/* + * 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(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_; + + 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_; + + 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); + 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..368bd5041a3 --- /dev/null +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_jpxy2mgrs.cpp @@ -0,0 +1,276 @@ +/* + * 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; +}; + +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 + "_" + 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; + 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 + "_" + 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; + 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 + "_" + 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; + 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; +} 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..df1a6fcbf1a --- /dev/null +++ b/ros/src/util/packages/map_tools/nodes/coordinate_transformer/pcd_latlon2mgrs.cpp @@ -0,0 +1,267 @@ +/* + * 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; +}; + +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 + "_" + 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; + 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 + "_" + 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; + 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 + "_" + 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; + 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/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