From 289ea9833dafbed2098bf16092a70cd0af6e47de Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Fri, 9 Feb 2024 17:20:14 +0900 Subject: [PATCH] feat(lanelet2_map_preprocessor): port lanelet2_map_preprocessor from Autoware.Universe Signed-off-by: mitsudome-r --- lanelet2_map_preprocessor/CMakeLists.txt | 33 +++ .../launch/fix_z_value_by_pcd.launch.xml | 12 ++ .../launch/transform_maps.launch.xml | 26 +++ lanelet2_map_preprocessor/package.xml | 23 ++ .../src/fix_lane_change_tags.cpp | 109 ++++++++++ .../src/fix_z_value_by_pcd.cpp | 169 +++++++++++++++ .../src/merge_close_lines.cpp | 202 ++++++++++++++++++ .../src/merge_close_points.cpp | 122 +++++++++++ .../src/remove_unreferenced_geometry.cpp | 99 +++++++++ .../src/transform_maps.cpp | 149 +++++++++++++ 10 files changed, 944 insertions(+) create mode 100644 lanelet2_map_preprocessor/CMakeLists.txt create mode 100644 lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml create mode 100644 lanelet2_map_preprocessor/launch/transform_maps.launch.xml create mode 100644 lanelet2_map_preprocessor/package.xml create mode 100644 lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp create mode 100644 lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp create mode 100644 lanelet2_map_preprocessor/src/merge_close_lines.cpp create mode 100644 lanelet2_map_preprocessor/src/merge_close_points.cpp create mode 100644 lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp create mode 100644 lanelet2_map_preprocessor/src/transform_maps.cpp diff --git a/lanelet2_map_preprocessor/CMakeLists.txt b/lanelet2_map_preprocessor/CMakeLists.txt new file mode 100644 index 00000000..8a6b16c0 --- /dev/null +++ b/lanelet2_map_preprocessor/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.14) +project(lanelet2_map_preprocessor) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PCL REQUIRED COMPONENTS common io kdtree) + +include_directories( + include + SYSTEM + ${PCL_INCLUDE_DIRS} +) + +link_libraries( + ${PCL_LIBRARIES} +) + +ament_auto_add_executable(fix_z_value_by_pcd src/fix_z_value_by_pcd.cpp) +ament_auto_add_executable(transform_maps src/transform_maps.cpp) +ament_auto_add_executable(merge_close_lines src/merge_close_lines.cpp) +ament_auto_add_executable(merge_close_points src/merge_close_points.cpp) +ament_auto_add_executable(remove_unreferenced_geometry src/remove_unreferenced_geometry.cpp) +ament_auto_add_executable(fix_lane_change_tags src/fix_lane_change_tags.cpp) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml b/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml new file mode 100644 index 00000000..82021ff2 --- /dev/null +++ b/lanelet2_map_preprocessor/launch/fix_z_value_by_pcd.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/lanelet2_map_preprocessor/launch/transform_maps.launch.xml b/lanelet2_map_preprocessor/launch/transform_maps.launch.xml new file mode 100644 index 00000000..902ca982 --- /dev/null +++ b/lanelet2_map_preprocessor/launch/transform_maps.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/lanelet2_map_preprocessor/package.xml b/lanelet2_map_preprocessor/package.xml new file mode 100644 index 00000000..79fce94f --- /dev/null +++ b/lanelet2_map_preprocessor/package.xml @@ -0,0 +1,23 @@ + + + + lanelet2_map_preprocessor + 0.1.0 + The lanelet2_map_preprocessor package + Ryohsuke Mitsudome + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + lanelet2_extension + libpcl-all-dev + rclcpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp b/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp new file mode 100644 index 00000000..89003810 --- /dev/null +++ b/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp @@ -0,0 +1,109 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +using lanelet::utils::getId; +using lanelet::utils::to2D; + +bool loadLaneletMap( + const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, + lanelet::Projector & projector) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors); + + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadLaneletMap"), error); + } + if (!errors.empty()) { + return false; + } + std::cout << "Loaded Lanelet2 map" << std::endl; + return true; +} + +bool exists(std::unordered_set & set, lanelet::Id element) +{ + return std::find(set.begin(), set.end(), element) != set.end(); +} + +lanelet::Lanelets convertToVector(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::Lanelets lanelets; + for (lanelet::Lanelet lanelet : lanelet_map_ptr->laneletLayer) { + lanelets.push_back(lanelet); + } + return lanelets; +} +void fixTags(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + auto lanelets = convertToVector(lanelet_map_ptr); + lanelet::traffic_rules::TrafficRulesPtr trafficRules = + lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Vehicle); + lanelet::routing::RoutingGraphUPtr routingGraph = + lanelet::routing::RoutingGraph::build(*lanelet_map_ptr, *trafficRules); + + for (auto & llt : lanelets) { + if (!routingGraph->conflicting(llt).empty()) { + continue; + } + llt.attributes().erase("turn_direction"); + if (!!routingGraph->adjacentRight(llt)) { + llt.rightBound().attributes()["lane_change"] = "yes"; + } + if (!!routingGraph->adjacentLeft(llt)) { + llt.leftBound().attributes()["lane_change"] = "yes"; + } + } +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node = rclcpp::Node::make_shared("fix_lane_change_tags"); + + const auto llt_map_path = node->declare_parameter("llt_map_path"); + const auto output_path = node->declare_parameter("output_path"); + + lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); + lanelet::projection::MGRSProjector projector; + + if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + + fixTags(llt_map_ptr); + lanelet::write(output_path, *llt_map_ptr, projector); + + rclcpp::shutdown(); + + return 0; +} diff --git a/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp b/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp new file mode 100644 index 00000000..b0eca472 --- /dev/null +++ b/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp @@ -0,0 +1,169 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +bool loadLaneletMap( + const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, + lanelet::Projector & projector) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors); + + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadLaneletMap"), error); + } + if (!errors.empty()) { + return false; + } + std::cout << "Loaded Lanelet2 map" << std::endl; + return true; +} + +bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) +{ + if (pcl::io::loadPCDFile(pcd_map_path, *pcd_map_ptr) == -1) { //* load the file + RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadPCDMap"), "Couldn't read file: " << pcd_map_path); + return false; + } + std::cout << "Loaded " << pcd_map_ptr->width * pcd_map_ptr->height << " data points." + << std::endl; + return true; +} + +double getMinHeightAroundPoint( + const pcl::PointCloud::Ptr & pcd_map_ptr, + const pcl::KdTreeFLANN & kdtree, const pcl::PointXYZ & search_pt, + const double search_radius3d, const double search_radius2d) +{ + std::vector pointIdxRadiusSearch; + std::vector pointRadiusSquaredDistance; + if ( + kdtree.radiusSearch( + search_pt, search_radius3d, pointIdxRadiusSearch, pointRadiusSquaredDistance) <= 0) { + std::cout << "no points found within 3d radius " << search_radius3d << std::endl; + return search_pt.z; + } + + double min_height = std::numeric_limits::max(); + bool found = false; + + for (std::size_t i = 0; i < pointIdxRadiusSearch.size(); i++) { + std::size_t pt_idx = pointIdxRadiusSearch.at(i); + const auto pt = pcd_map_ptr->points.at(pt_idx); + if (pt.z > min_height) { + continue; + } + double distance2d = std::hypot(pt.x - search_pt.x, pt.y - search_pt.y); + if (distance2d < search_radius2d) { + found = true; + min_height = pt.z; + } + } + if (!found) { + min_height = search_pt.z; + } + return min_height; +} + +bool exists(std::unordered_set & set, lanelet::Id element) +{ + return std::find(set.begin(), set.end(), element) != set.end(); +} + +void adjustHeight( + const pcl::PointCloud::Ptr & pcd_map_ptr, lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + std::unordered_set done; + double search_radius2d = 0.5; + double search_radius3d = 10; + + pcl::KdTreeFLANN kdtree; + kdtree.setInputCloud(pcd_map_ptr); + + for (lanelet::Lanelet & llt : lanelet_map_ptr->laneletLayer) { + for (lanelet::Point3d & pt : llt.leftBound()) { + if (exists(done, pt.id())) { + continue; + } + pcl::PointXYZ pcl_pt; + pcl_pt.x = pt.x(); + pcl_pt.y = pt.y(); + pcl_pt.z = pt.z(); + double min_height = + getMinHeightAroundPoint(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); + std::cout << "moving from " << pt.z() << " to " << min_height << std::endl; + pt.z() = min_height; + done.insert(pt.id()); + } + for (lanelet::Point3d & pt : llt.rightBound()) { + if (exists(done, pt.id())) { + continue; + } + pcl::PointXYZ pcl_pt; + pcl_pt.x = pt.x(); + pcl_pt.y = pt.y(); + pcl_pt.z = pt.z(); + double min_height = + getMinHeightAroundPoint(pcd_map_ptr, kdtree, pcl_pt, search_radius3d, search_radius2d); + std::cout << "moving from " << pt.z() << " to " << min_height << std::endl; + pt.z() = min_height; + done.insert(pt.id()); + } + } +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node = rclcpp::Node::make_shared("lanelet_map_height_adjuster"); + + const auto llt_map_path = node->declare_parameter("llt_map_path"); + const auto pcd_map_path = node->declare_parameter("pcd_map_path"); + const auto llt_output_path = node->declare_parameter("llt_output_path"); + + lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); + lanelet::projection::MGRSProjector projector; + + pcl::PointCloud::Ptr pcd_map_ptr(new pcl::PointCloud); + + if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + if (!loadPCDMap(pcd_map_path, pcd_map_ptr)) { + return EXIT_FAILURE; + } + + adjustHeight(pcd_map_ptr, llt_map_ptr); + lanelet::write(llt_output_path, *llt_map_ptr, projector); + + rclcpp::shutdown(); + + return 0; +} diff --git a/lanelet2_map_preprocessor/src/merge_close_lines.cpp b/lanelet2_map_preprocessor/src/merge_close_lines.cpp new file mode 100644 index 00000000..a9e45b3b --- /dev/null +++ b/lanelet2_map_preprocessor/src/merge_close_lines.cpp @@ -0,0 +1,202 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +using lanelet::utils::getId; +using lanelet::utils::to2D; + +bool loadLaneletMap( + const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, + lanelet::Projector & projector) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors); + + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadLaneletMap"), error); + } + if (!errors.empty()) { + return false; + } + std::cout << "Loaded Lanelet2 map" << std::endl; + return true; +} + +bool exists(std::unordered_set & set, lanelet::Id element) +{ + return std::find(set.begin(), set.end(), element) != set.end(); +} + +lanelet::LineStrings3d convertLineLayerToLineStrings(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::LineStrings3d lines; + for (const lanelet::LineString3d & line : lanelet_map_ptr->lineStringLayer) { + lines.push_back(line); + } + return lines; +} + +lanelet::ConstPoint3d get3DPointFrom2DArcLength( + const lanelet::ConstLineString3d & line, const double s) +{ + double accumulated_distance2d = 0; + if (line.size() < 2) { + return lanelet::Point3d(); + } + auto prev_pt = line.front(); + for (size_t i = 1; i < line.size(); i++) { + const auto & pt = line[i]; + double distance2d = lanelet::geometry::distance2d(to2D(prev_pt), to2D(pt)); + if (accumulated_distance2d + distance2d >= s) { + double ratio = (s - accumulated_distance2d) / distance2d; + auto interpolated_pt = prev_pt.basicPoint() * (1 - ratio) + pt.basicPoint() * ratio; + std::cout << interpolated_pt << std::endl; + return lanelet::ConstPoint3d( + lanelet::utils::getId(), interpolated_pt.x(), interpolated_pt.y(), interpolated_pt.z()); + } + accumulated_distance2d += distance2d; + prev_pt = pt; + } + RCLCPP_ERROR(rclcpp::get_logger("merge_close_lines"), "interpolation failed"); + return lanelet::ConstPoint3d(); +} + +double getLineLength(const lanelet::ConstLineString3d & line) +{ + return boost::geometry::length(line.basicLineString()); +} + +bool areLinesSame( + const lanelet::ConstLineString3d & line1, const lanelet::ConstLineString3d & line2) +{ + bool same_ends = false; + if (line1.front() == line2.front() && line1.back() == line2.back()) { + same_ends = true; + } + if (line1.front() == line2.back() && line1.back() == line2.front()) { + same_ends = true; + } + if (!same_ends) { + return false; + } + + double sum_distance = 0; + for (const auto & pt : line1) { + sum_distance += boost::geometry::distance(pt.basicPoint(), line2); + } + for (const auto & pt : line2) { + sum_distance += boost::geometry::distance(pt.basicPoint(), line1); + } + + double avg_distance = sum_distance / (line1.size() + line2.size()); + std::cout << line1 << " " << line2 << " " << avg_distance << std::endl; + if (avg_distance < 1.0) { + return true; + } else { + return false; + } +} + +lanelet::BasicPoint3d getClosestPointOnLine( + const lanelet::BasicPoint3d & search_point, const lanelet::ConstLineString3d & line) +{ + auto arc_coordinate = lanelet::geometry::toArcCoordinates(to2D(line), to2D(search_point)); + std::cout << arc_coordinate.length << " " << arc_coordinate.distance << std::endl; + return get3DPointFrom2DArcLength(line, arc_coordinate.length).basicPoint(); +} + +lanelet::LineString3d mergeTwoLines( + const lanelet::LineString3d & line1, const lanelet::ConstLineString3d & line2) +{ + lanelet::Points3d new_points; + for (const auto & p1 : line1) { + lanelet::BasicPoint3d p1_basic_point = p1.basicPoint(); + lanelet::BasicPoint3d p2_basic_point = getClosestPointOnLine(p1, line2); + lanelet::BasicPoint3d new_basic_point = (p1_basic_point + p2_basic_point) / 2; + lanelet::Point3d new_point(lanelet::utils::getId(), new_basic_point); + new_points.push_back(new_point); + } + return lanelet::LineString3d(lanelet::utils::getId(), new_points); +} + +void copyData(lanelet::LineString3d & dst, lanelet::LineString3d & src) +{ + lanelet::Points3d points; + dst.clear(); + for (lanelet::Point3d & pt : src) { + dst.push_back(pt); + } +} + +void mergeLines(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + auto lines = convertLineLayerToLineStrings(lanelet_map_ptr); + + for (size_t i = 0; i < lines.size(); i++) { + auto line_i = lines.at(i); + for (size_t j = 0; j < i; j++) { + auto line_j = lines.at(j); + if (areLinesSame(line_i, line_j)) { + auto merged_line = mergeTwoLines(line_i, line_j); + copyData(line_i, merged_line); + copyData(line_j, merged_line); + line_i.setId(line_j.id()); + std::cout << line_j << " " << line_i << std::endl; + // lanelet_map_ptr->add(merged_line); + for (lanelet::Point3d & pt : merged_line) { + lanelet_map_ptr->add(pt); + } + break; + } + } + } +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node = rclcpp::Node::make_shared("merge_close_lines"); + + const auto llt_map_path = node->declare_parameter("llt_map_path"); + const auto output_path = node->declare_parameter("output_path"); + + lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); + lanelet::projection::MGRSProjector projector; + + if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + + mergeLines(llt_map_ptr); + lanelet::write(output_path, *llt_map_ptr, projector); + + rclcpp::shutdown(); + + return 0; +} diff --git a/lanelet2_map_preprocessor/src/merge_close_points.cpp b/lanelet2_map_preprocessor/src/merge_close_points.cpp new file mode 100644 index 00000000..ab77b184 --- /dev/null +++ b/lanelet2_map_preprocessor/src/merge_close_points.cpp @@ -0,0 +1,122 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +bool loadLaneletMap( + const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, + lanelet::Projector & projector) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors); + + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadLaneletMap"), error); + } + if (!errors.empty()) { + return false; + } + std::cout << "Loaded Lanelet2 map" << std::endl; + return true; +} + +bool exists(std::unordered_set & set, lanelet::Id element) +{ + return std::find(set.begin(), set.end(), element) != set.end(); +} + +lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::Points3d points; + for (const lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { + points.push_back(pt); + } + return points; +} + +// lanelet::LineString3d mergeClosePoints(const lanelet::ConstLineString3d& line1, const +// lanelet::ConstLineString3d& line2) +// { +// lanelet::Points3d new_points; +// for (const auto& p1 : line1) +// { +// p1_basic_point = p1.basicPoint(); +// lanelet::BasicPoint3d p2 = getClosestPointOnLine(line2, p1); +// lanelet::BasicPoint3d new_basic_point = (p1_basic_point + p2_basic_point)/2; +// lanelet::Point3d new_point(lanelet::utils::getId(), new_basic_point); +// new_points.push_back(new_point); +// } +// return lanelet::LineString3d(lanelet::utils::getId(), new_points); +// } + +void mergePoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + const auto & points = convertPointsLayerToPoints(lanelet_map_ptr); + + for (size_t i = 0; i < points.size(); i++) { + auto point_i = points.at(i); + for (size_t j = 0; j < i; j++) { + auto point_j = points.at(j); + + double distance = boost::geometry::distance(point_i, point_j); + if (distance < 0.1) { + const auto new_point = (point_i.basicPoint() + point_j.basicPoint()) / 2; + // const auto new_pt3d = lanelet::Point3d(lanelet::utils::getId(), new_point); + point_i.x() = new_point.x(); + point_i.y() = new_point.y(); + point_i.z() = new_point.z(); + point_j.x() = new_point.x(); + point_j.y() = new_point.y(); + point_j.z() = new_point.z(); + point_i.setId(point_j.id()); + } + } + } +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node = rclcpp::Node::make_shared("merge_close_points"); + + const auto llt_map_path = node->declare_parameter("llt_map_path"); + const auto output_path = node->declare_parameter("output_path"); + + lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); + lanelet::projection::MGRSProjector projector; + + if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + + mergePoints(llt_map_ptr); + lanelet::write(output_path, *llt_map_ptr, projector); + + rclcpp::shutdown(); + + return 0; +} diff --git a/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp b/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp new file mode 100644 index 00000000..ca488b3a --- /dev/null +++ b/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp @@ -0,0 +1,99 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +bool loadLaneletMap( + const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, + lanelet::Projector & projector) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors); + + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadLaneletMap"), error); + } + if (!errors.empty()) { + return false; + } + std::cout << "Loaded Lanelet2 map" << std::endl; + return true; +} + +bool exists(std::unordered_set & set, lanelet::Id element) +{ + return std::find(set.begin(), set.end(), element) != set.end(); +} + +lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::Points3d points; + for (const lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { + points.push_back(pt); + } + return points; +} + +lanelet::LineStrings3d convertLineLayerToLineStrings(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::LineStrings3d lines; + for (const lanelet::LineString3d & line : lanelet_map_ptr->lineStringLayer) { + lines.push_back(line); + } + return lines; +} + +void removeUnreferencedGeometry(lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + lanelet::LaneletMapPtr new_map(new lanelet::LaneletMap); + for (auto llt : lanelet_map_ptr->laneletLayer) { + new_map->add(llt); + } + lanelet_map_ptr = new_map; +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node = rclcpp::Node::make_shared("remove_unreferenced_geometry"); + + const auto llt_map_path = node->declare_parameter("llt_map_path"); + const auto output_path = node->declare_parameter("output_path"); + + lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); + lanelet::projection::MGRSProjector projector; + + if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + removeUnreferencedGeometry(llt_map_ptr); + lanelet::write(output_path, *llt_map_ptr, projector); + + rclcpp::shutdown(); + + return 0; +} diff --git a/lanelet2_map_preprocessor/src/transform_maps.cpp b/lanelet2_map_preprocessor/src/transform_maps.cpp new file mode 100644 index 00000000..987f559b --- /dev/null +++ b/lanelet2_map_preprocessor/src/transform_maps.cpp @@ -0,0 +1,149 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +bool loadLaneletMap( + const std::string & llt_map_path, lanelet::LaneletMapPtr & lanelet_map_ptr, + lanelet::Projector & projector) +{ + lanelet::LaneletMapPtr lanelet_map; + lanelet::ErrorMessages errors; + lanelet_map_ptr = lanelet::load(llt_map_path, "autoware_osm_handler", projector, &errors); + + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadLaneletMap"), error); + } + if (!errors.empty()) { + return false; + } + std::cout << "Loaded Lanelet2 map" << std::endl; + return true; +} + +bool loadPCDMap(const std::string & pcd_map_path, pcl::PointCloud::Ptr & pcd_map_ptr) +{ + if (pcl::io::loadPCDFile(pcd_map_path, *pcd_map_ptr) == -1) { //* load the file + RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadPCDMap"), "Couldn't read file: " << pcd_map_path); + return false; + } + std::cout << "Loaded " << pcd_map_ptr->width * pcd_map_ptr->height << " data points." + << std::endl; + return true; +} + +void transformMaps( + pcl::PointCloud::Ptr & pcd_map_ptr, lanelet::LaneletMapPtr & lanelet_map_ptr, + const Eigen::Affine3d affine) +{ + { + for (lanelet::Point3d & pt : lanelet_map_ptr->pointLayer) { + Eigen::Vector3d eigen_pt(pt.x(), pt.y(), pt.z()); + auto transformed_pt = affine * eigen_pt; + pt.x() = transformed_pt.x(); + pt.y() = transformed_pt.y(); + pt.z() = transformed_pt.z(); + } + } + + { + for (auto & pt : pcd_map_ptr->points) { + Eigen::Vector3d eigen_pt(pt.x, pt.y, pt.z); + auto transformed_pt = affine * eigen_pt; + pt.x = transformed_pt.x(); + pt.y = transformed_pt.y(); + pt.z = transformed_pt.z(); + } + } +} + +Eigen::Affine3d createAffineMatrixFromXYZRPY( + const double x, const double y, const double z, const double roll, const double pitch, + const double yaw) +{ + double roll_rad = roll * M_PI / 180.0; + double pitch_rad = pitch * M_PI / 180.0; + double yaw_rad = yaw * M_PI / 180.0; + + Eigen::Translation trans(x, y, z); + Eigen::Matrix3d rot; + rot = Eigen::AngleAxisd(yaw_rad, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(pitch_rad, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(roll_rad, Eigen::Vector3d::UnitX()); + return trans * rot; +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto node = rclcpp::Node::make_shared("transform_maps"); + + const auto llt_map_path = node->declare_parameter("llt_map_path"); + const auto pcd_map_path = node->declare_parameter("pcd_map_path"); + const auto llt_output_path = node->declare_parameter("llt_output_path"); + const auto pcd_output_path = node->declare_parameter("pcd_output_path"); + const auto x = node->declare_parameter("x", 0.0); + const auto y = node->declare_parameter("y", 0.0); + const auto z = node->declare_parameter("z", 0.0); + const auto roll = node->declare_parameter("roll", 0.0); + const auto pitch = node->declare_parameter("pitch", 0.0); + const auto yaw = node->declare_parameter("yaw", 0.0); + + std::cout << "transforming maps with following parameters" << std::endl + << "x " << x << std::endl + << "y " << y << std::endl + << "z " << z << std::endl + << "roll " << roll << std::endl + << "pitch " << pitch << std::endl + << "yaw " << yaw << std::endl; + + lanelet::LaneletMapPtr llt_map_ptr(new lanelet::LaneletMap); + lanelet::projection::MGRSProjector projector; + + pcl::PointCloud::Ptr pcd_map_ptr(new pcl::PointCloud); + + if (!loadLaneletMap(llt_map_path, llt_map_ptr, projector)) { + return EXIT_FAILURE; + } + if (!loadPCDMap(pcd_map_path, pcd_map_ptr)) { + return EXIT_FAILURE; + } + Eigen::Affine3d affine = createAffineMatrixFromXYZRPY(x, y, z, roll, pitch, yaw); + + const auto mgrs_grid = + node->declare_parameter("mgrs_grid", projector.getProjectedMGRSGrid()); + std::cout << "using mgrs grid: " << mgrs_grid << std::endl; + + transformMaps(pcd_map_ptr, llt_map_ptr, affine); + lanelet::write(llt_output_path, *llt_map_ptr, projector); + pcl::io::savePCDFileBinary(pcd_output_path, *pcd_map_ptr); + + rclcpp::shutdown(); + + return 0; +}