Feat: Add PCD to MGRS conversion service (/convert_mgrs) #24
There are no files selected for viewing
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,98 @@ | ||
| #include "utm_to_mgrs_converter.h" | ||
| #include <GeographicLib/MGRS.hpp> | ||
| #include <GeographicLib/UTMUPS.hpp> | ||
| #include <GeographicLib/Geoid.hpp> | ||
| #include <pcl/point_cloud.h> | ||
| #include <pcl/point_types.h> | ||
| #include <pcl_conversions/pcl_conversions.h> | ||
| #include <ros/ros.h> | ||
| #include <iostream> | ||
|
|
||
| GNSSStat convertUTM2MGRS(GNSSStat gnss_stat_utm, const MGRSPrecision precision) { | ||
| constexpr int GZD_ID_size = 5; // size of header like "53SPU" | ||
|
|
||
| GNSSStat mgrs = gnss_stat_utm; | ||
| mgrs.coordinate_system = CoordinateSystem::MGRS; | ||
| try { | ||
| std::string mgrs_code; | ||
| GeographicLib::MGRS::Forward( | ||
| gnss_stat_utm.zone, gnss_stat_utm.northup, gnss_stat_utm.x, gnss_stat_utm.y, gnss_stat_utm.latitude, static_cast<int>(precision), mgrs_code); | ||
| mgrs.mgrs_zone = std::string(mgrs_code.substr(0, GZD_ID_size)); | ||
| mgrs.x = std::stod(mgrs_code.substr(GZD_ID_size, static_cast<int>(precision))) * | ||
| std::pow(10, static_cast<int>(MGRSPrecision::_1_METER) - static_cast<int>(precision)); // set unit as [m] | ||
| mgrs.y = std::stod(mgrs_code.substr(GZD_ID_size + static_cast<int>(precision), static_cast<int>(precision))) * | ||
| std::pow(10, static_cast<int>(MGRSPrecision::_1_METER) - static_cast<int>(precision)); // set unit as [m] | ||
| mgrs.z = gnss_stat_utm.z; // set unit as [m] | ||
| } catch (const GeographicLib::GeographicErr & err) { | ||
| ROS_ERROR_STREAM("Failed to convert from UTM to MGRS: " << err.what()); | ||
| } | ||
| return mgrs; | ||
| } | ||
|
|
||
| bool convertPCDToMGRS(const std::string& input_file_path, const std::string& output_file_path, | ||
| double map_origin_northing, double map_origin_easting, double map_origin_height) { | ||
| pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloud(new pcl::PointCloud<pcl::PointXYZI>); | ||
| if (pcl::io::loadPCDFile<pcl::PointXYZI>(input_file_path, *inputCloud) == -1) { | ||
| ROS_ERROR_STREAM("Couldn't read file " << input_file_path); | ||
| return false; | ||
| } | ||
|
|
||
| double counter = 0; | ||
| GNSSStat gnss_stat_utm; | ||
| GNSSStat gnss_stat_mgrs; | ||
|
|
||
| pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_mgrs(new pcl::PointCloud<pcl::PointXYZI>); | ||
| cloud_mgrs->width = inputCloud->width; | ||
| cloud_mgrs->height = inputCloud->height; | ||
| cloud_mgrs->is_dense = inputCloud->is_dense; | ||
|
|
||
| for (long unsigned int i = 0; i < inputCloud->points.size(); i++) { | ||
| pcl::PointXYZI point = inputCloud->points[i]; | ||
|
|
||
| // convert local UTM to global UTM coordinates | ||
| gnss_stat_utm.x = point.x + map_origin_easting; | ||
| gnss_stat_utm.y = point.y + map_origin_northing; | ||
| gnss_stat_utm.z = point.z + map_origin_height; | ||
| gnss_stat_utm.coordinate_system = CoordinateSystem::UTM; | ||
| gnss_stat_utm.zone = 35; | ||
|
There was a problem hiding this comment.
When the map origin is in any UTM zone other than 35, the service reverses the configured easting/northing through zone 35 and then emits an MGRS code for the wrong part of the world, so the converted map is geographically displaced. Since Useful? React with 👍 / 👎. |
||
| gnss_stat_utm.northup = true; | ||
|
|
||
| // convert latitude and longitude from UTM | ||
| GeographicLib::UTMUPS::Reverse(gnss_stat_utm.zone, gnss_stat_utm.northup, gnss_stat_utm.x, gnss_stat_utm.y, gnss_stat_utm.latitude, gnss_stat_utm.longitude); | ||
|
|
||
| gnss_stat_utm.altitude = gnss_stat_utm.z; | ||
|
|
||
| // convert height from ellipsoid to orthometric | ||
| double OrthometricHeight{0.0}; | ||
| try { | ||
| GeographicLib::Geoid egm2008("egm2008-1"); | ||
| OrthometricHeight = egm2008.ConvertHeight( | ||
| gnss_stat_utm.latitude, gnss_stat_utm.longitude, gnss_stat_utm.altitude, | ||
| GeographicLib::Geoid::ELLIPSOIDTOGEOID); | ||
| } catch (const GeographicLib::GeographicErr & err) { | ||
| OrthometricHeight = gnss_stat_utm.altitude; | ||
| } | ||
|
|
||
| // convert global UTM to MGRS | ||
| gnss_stat_mgrs = convertUTM2MGRS(gnss_stat_utm, MGRSPrecision::_100MICRO_METER); | ||
|
|
||
| //create new pointcloud with mgrs coordinates | ||
| pcl::PointXYZI point_mgrs; | ||
| point_mgrs.x = gnss_stat_mgrs.x; | ||
| point_mgrs.y = gnss_stat_mgrs.y; | ||
| point_mgrs.z = OrthometricHeight; | ||
| point_mgrs.intensity = point.intensity; | ||
| cloud_mgrs->points.push_back(std::move(point_mgrs)); | ||
|
|
||
| if (counter == 1000000) { | ||
| ROS_INFO("continue."); | ||
| counter = 0; | ||
| } | ||
| counter++; | ||
| } | ||
|
|
||
| //save pcd file | ||
| pcl::io::savePCDFileASCII(output_file_path, *cloud_mgrs); | ||
| ROS_INFO_STREAM("Saved " << cloud_mgrs->points.size() << " data points to " << output_file_path); | ||
| return true; | ||
| } | ||
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,46 @@ | ||
| #ifndef MS_MAPPING_UTM_TO_MGRS_CONVERTER_H | ||
| #define MS_MAPPING_UTM_TO_MGRS_CONVERTER_H | ||
|
|
||
| #include <string> | ||
|
|
||
| enum class MGRSPrecision { | ||
| _1_METER = 5, | ||
| _100MICRO_METER = 9, | ||
| }; | ||
|
|
||
| enum class CoordinateSystem { | ||
| UTM = 0, | ||
| MGRS = 1, | ||
| }; | ||
|
|
||
| struct GNSSStat { | ||
| GNSSStat() | ||
| : coordinate_system(CoordinateSystem::MGRS), | ||
| northup(true), | ||
| zone(0), | ||
| mgrs_zone(""), | ||
| x(0), | ||
| y(0), | ||
| z(0), | ||
| latitude(0), | ||
| longitude(0), | ||
| altitude(0) {} | ||
|
|
||
| CoordinateSystem coordinate_system; | ||
| bool northup; | ||
| int zone; | ||
| std::string mgrs_zone; | ||
| double x; | ||
| double y; | ||
| double z; | ||
| double latitude; | ||
| double longitude; | ||
| double altitude; | ||
| }; | ||
|
|
||
| GNSSStat convertUTM2MGRS(GNSSStat gnss_stat_utm, const MGRSPrecision precision); | ||
|
|
||
| bool convertPCDToMGRS(const std::string& input_file_path, const std::string& output_file_path, | ||
| double map_origin_northing, double map_origin_easting, double map_origin_height); | ||
|
|
||
| #endif // MS_MAPPING_UTM_TO_MGRS_CONVERTER_H |
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
When
save_directoryis configured without a trailing slash, including theLoadRosParamsdefault,DataSavernormalizes the base path before writingfinal_map_lidar.pcd, but this service concatenates the rawsaveDirectoryandsequence. In that configuration/save_mapwrites under<base>/<sequence>/...while/convert_mgrslooks under<base><sequence>/..., so conversion fails even though the map was saved successfully.Useful? React with 👍 / 👎.