diff --git a/config/sdf_mapping_ucsd_fah_2d.yaml b/config/sdf_mapping_ucsd_fah_2d.yaml index 7ee49d4..953bde9 100644 --- a/config/sdf_mapping_ucsd_fah_2d.yaml +++ b/config/sdf_mapping_ucsd_fah_2d.yaml @@ -43,7 +43,7 @@ surface_mapping: valid_range_max: 30 angle_min: -3.141592653 # -2.3561944901923448 angle_max: 3.141592653 # 2.3561944901923448 - num_rays: 360 + num_rays: 1024 discontinuity_factor: 10 rolling_diff_discount: 0.9 min_partition_size: 5 diff --git a/config/surface_mapping_2d.yaml b/config/surface_mapping_2d.yaml index 273f7a8..72142b5 100644 --- a/config/surface_mapping_2d.yaml +++ b/config/surface_mapping_2d.yaml @@ -11,7 +11,7 @@ sensor_gp: valid_range_max: 30 angle_min: -3.141592653 # -2.3561944901923448 angle_max: 3.141592653 # 2.3561944901923448 - num_rays: 360 + num_rays: 1024 discontinuity_factor: 10 rolling_diff_discount: 0.9 min_partition_size: 5 diff --git a/include/erl_sdf_mapping/gp_occ_surface_mapping_2d.hpp b/include/erl_sdf_mapping/gp_occ_surface_mapping_2d.hpp index 2d6aa65..792f29a 100644 --- a/include/erl_sdf_mapping/gp_occ_surface_mapping_2d.hpp +++ b/include/erl_sdf_mapping/gp_occ_surface_mapping_2d.hpp @@ -41,7 +41,7 @@ namespace erl::sdf_mapping { // clang-format on } - [[nodiscard]] std::shared_ptr + [[nodiscard]] std::shared_ptr GetSetting() const { return m_setting_; } diff --git a/include/erl_sdf_mapping/gp_sdf_mapping_2d.hpp b/include/erl_sdf_mapping/gp_sdf_mapping_2d.hpp index bd35637..c73aaf2 100644 --- a/include/erl_sdf_mapping/gp_sdf_mapping_2d.hpp +++ b/include/erl_sdf_mapping/gp_sdf_mapping_2d.hpp @@ -2,6 +2,7 @@ #include "abstract_surface_mapping_2d.hpp" #include "gp_sdf_mapping_base_setting.hpp" +#include "gp_occ_surface_mapping_2d.hpp" #include "sdf_gp.hpp" #include "erl_common/csv.hpp" @@ -22,7 +23,7 @@ namespace erl::sdf_mapping { struct Setting : common::Yamlable { std::string surface_mapping_type = "erl::sdf_mapping::GpOccSurfaceMapping2D"; std::string surface_mapping_setting_type = "erl::sdf_mapping::GpOccSurfaceMapping2D::Setting"; - std::shared_ptr surface_mapping = nullptr; + std::shared_ptr surface_mapping = std::make_shared(); }; inline static const volatile bool kSettingRegistered = common::YamlableBase::Register(); @@ -59,7 +60,7 @@ namespace erl::sdf_mapping { std::shared_ptr m_setting_ = std::make_shared(); std::mutex m_mutex_; - std::shared_ptr m_surface_mapping_ = nullptr; // for getting surface points, racing condition. + std::shared_ptr m_surface_mapping_ = nullptr; // for getting surface points, racing condition. KeyGpMap m_gp_map_ = {}; // for getting GP from Quadtree key, racing condition. KeyVector m_affected_clusters_ = {}; // stores clusters that are to be updated after a new observation KeyQueueMap m_cluster_queue_keys_ = {}; // caching keys of clusters in the queue to be updated @@ -109,7 +110,7 @@ namespace erl::sdf_mapping { return m_setting_; } - [[nodiscard]] std::shared_ptr + [[nodiscard]] std::shared_ptr GetSurfaceMapping() const { return m_surface_mapping_; } @@ -204,7 +205,7 @@ struct YAML::convert { if (!convert::decode(node, rhs)) { return false; } rhs.surface_mapping_type = node["surface_mapping_type"].as(); rhs.surface_mapping_setting_type = node["surface_mapping_setting_type"].as(); - using SettingBase = erl::sdf_mapping::AbstractSurfaceMapping2D::Setting; + using SettingBase = erl::sdf_mapping::GpOccSurfaceMapping2D::Setting; rhs.surface_mapping = SettingBase::Create(rhs.surface_mapping_setting_type); if (rhs.surface_mapping == nullptr) { ERL_WARN("Failed to decode surface_mapping of type: {}", rhs.surface_mapping_setting_type); diff --git a/include/erl_sdf_mapping/surface_mapping_quadtree_node.hpp b/include/erl_sdf_mapping/surface_mapping_quadtree_node.hpp index 64dec0e..45a988f 100644 --- a/include/erl_sdf_mapping/surface_mapping_quadtree_node.hpp +++ b/include/erl_sdf_mapping/surface_mapping_quadtree_node.hpp @@ -42,7 +42,7 @@ namespace erl::sdf_mapping { [[nodiscard]] bool AllowMerge(const AbstractQuadtreeNode *other) const override { if (surface_data_index != static_cast(-1)) { return false; } - ERL_DEBUG_ASSERT(dynamic_cast(other) != nullptr, "other node is not SurfaceMappingOctreeNode."); + ERL_DEBUG_ASSERT(dynamic_cast(other) != nullptr, "other node is not SurfaceMappingQuadtreeNode."); if (const auto *other_node = reinterpret_cast(other); other_node->surface_data_index != static_cast(-1)) { return false; diff --git a/launch/sdf_mapping.launch b/launch/sdf_mapping.launch new file mode 100644 index 0000000..68e258a --- /dev/null +++ b/launch/sdf_mapping.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/src/gp_occ_surface_mapping_2d.cpp b/src/gp_occ_surface_mapping_2d.cpp index 96186bc..0903987 100644 --- a/src/gp_occ_surface_mapping_2d.cpp +++ b/src/gp_occ_surface_mapping_2d.cpp @@ -235,7 +235,7 @@ namespace erl::sdf_mapping { nodes_in_aabb.emplace_back(it.GetKey(), *it, std::nullopt); } -#pragma omp parallel for default(none) shared(nodes_in_aabb, max_sensor_range, sensor_pos, sensor_frame, g_print_mutex) +#pragma omp parallel for shared(nodes_in_aabb, max_sensor_range, sensor_pos, sensor_frame, g_print_mutex) for (auto &[node_key, node, new_key]: nodes_in_aabb) { const uint32_t cluster_level = m_setting_->cluster_level; const double sensor_range_var = m_setting_->sensor_gp->sensor_range_var; diff --git a/src/gp_occ_surface_mapping_3d.cpp b/src/gp_occ_surface_mapping_3d.cpp index 8d29b95..5517d22 100644 --- a/src/gp_occ_surface_mapping_3d.cpp +++ b/src/gp_occ_surface_mapping_3d.cpp @@ -231,7 +231,7 @@ namespace erl::sdf_mapping { nodes_in_aabb.emplace_back(it.GetKey(), *it, std::nullopt); } -#pragma omp parallel for default(none) shared(nodes_in_aabb, max_sensor_range, sensor_pos, sensor_frame, g_print_mutex) +#pragma omp parallel for shared(nodes_in_aabb, max_sensor_range, sensor_pos, sensor_frame, g_print_mutex) for (auto &[node_key, node, new_key]: nodes_in_aabb) { const uint32_t cluster_level = m_setting_->cluster_level; const double sensor_range_var = m_setting_->sensor_gp->sensor_range_var; diff --git a/src/gp_sdf_mapping_2d.cpp b/src/gp_sdf_mapping_2d.cpp index 5bf0996..30559b2 100644 --- a/src/gp_sdf_mapping_2d.cpp +++ b/src/gp_sdf_mapping_2d.cpp @@ -50,8 +50,7 @@ namespace erl::sdf_mapping { GpSdfMapping2D::GpSdfMapping2D(std::shared_ptr setting) : m_setting_(NotNull(std::move(setting), "setting is nullptr.")), - m_surface_mapping_( - AbstractSurfaceMapping::CreateSurfaceMapping(m_setting_->surface_mapping_type, m_setting_->surface_mapping)) { + m_surface_mapping_(std::make_shared(m_setting_->surface_mapping)) { ERL_ASSERTM(m_surface_mapping_ != nullptr, "surface_mapping is nullptr."); } @@ -403,7 +402,7 @@ namespace erl::sdf_mapping { skip_line(); if (has_surface_mapping) { if (m_surface_mapping_ == nullptr) { - m_surface_mapping_ = AbstractSurfaceMapping::CreateSurfaceMapping( + m_surface_mapping_ = AbstractSurfaceMapping::CreateSurfaceMapping( m_setting_->surface_mapping_type, m_setting_->surface_mapping); } diff --git a/src/ros/erl_sdf_mapping_node.cpp b/src/ros/erl_sdf_mapping_node.cpp index 86109d2..0de821e 100644 --- a/src/ros/erl_sdf_mapping_node.cpp +++ b/src/ros/erl_sdf_mapping_node.cpp @@ -11,6 +11,7 @@ #include "erl_geometry/occupancy_quadtree_drawer.hpp" // must make sure the tree is included before the drawer #include "erl_sdf_mapping/gp_occ_surface_mapping_2d.hpp" #include "erl_sdf_mapping/gp_sdf_mapping_2d.hpp" +#include "erl_geometry/lidar_frame_2d.hpp" #define BOOST_BIND_GLOBAL_PLACEHOLDERS #include #include @@ -72,43 +73,23 @@ class RosNode { (void) ros::Duration(1.0).sleep(); LoadParameters(); // load parameters from ros parameter server - m_surface_mapping_ = std::make_shared(m_surface_mapping_setting_); - m_sdf_mapping_ = std::make_shared(m_surface_mapping_, m_sdf_mapping_setting_); - if (m_params_.visualize_quadtree) { - m_quadtree_drawer_ = std::make_shared(m_quadtree_drawer_setting_); - m_quadtree_drawer_->SetDrawTreeCallback( - [&](const OccupancyQuadtreeDrawer *self, cv::Mat &img, erl::sdf_mapping::SurfaceMappingQuadtree::TreeIterator &it) { - unsigned int cluster_depth = m_surface_mapping_->GetQuadtree()->GetTreeDepth() - m_surface_mapping_->GetClusterLevel(); - auto grid_map_info = self->GetGridMapInfo(); - if (it->GetDepth() == cluster_depth) { - Eigen::Vector2i position_px = grid_map_info->MeterToPixelForPoints(Eigen::Vector2d(it.GetX(), it.GetY())); - cv::Point position_px_cv(position_px[0], position_px[1]); - cv::circle(img, position_px_cv, 2, cv::Scalar(0, 0, 255, 255), -1); // draw surface point - return; - } - if (it->GetSurfaceData() == nullptr) { return; } - Eigen::Vector2i position_px = grid_map_info->MeterToPixelForPoints(it->GetSurfaceData()->position); - cv::Point position_px_cv(position_px[0], position_px[1]); - cv::circle(img, cv::Point(position_px[0], position_px[1]), 1, cv::Scalar(0, 0, 255, 255), -1); // draw surface point - Eigen::Vector2i normal_px = grid_map_info->MeterToPixelForVectors(it->GetSurfaceData()->normal * 0.5); - cv::Point arrow_end_px(position_px[0] + normal_px[0], position_px[1] + normal_px[1]); - m_arrowed_lines_.emplace_back(position_px_cv, arrow_end_px); - }); - m_quadtree_drawer_->SetDrawLeafCallback( - [&](const OccupancyQuadtreeDrawer *self, cv::Mat &img, erl::sdf_mapping::SurfaceMappingQuadtree::LeafIterator &it) { - if (it->GetSurfaceData() == nullptr) { return; } - auto grid_map_info = self->GetGridMapInfo(); - Eigen::Vector2i position_px = grid_map_info->MeterToPixelForPoints(it->GetSurfaceData()->position); - cv::Point position_px_cv(position_px[0], position_px[1]); - cv::circle(img, position_px_cv, 1, cv::Scalar(0, 0, 255, 255), -1); // draw surface point - Eigen::Vector2i normal_px = grid_map_info->MeterToPixelForVectors(it->GetSurfaceData()->normal * 0.5); - cv::Point arrow_end_px(position_px[0] + normal_px[0], position_px[1] + normal_px[1]); - m_arrowed_lines_.emplace_back(position_px_cv, arrow_end_px); - }); - auto grid_map_info = m_quadtree_drawer_->GetGridMapInfo(); - m_cv_image_ = cv::Mat(grid_map_info->Height(), grid_map_info->Width(), CV_8UC4, cv::Scalar(128, 128, 128, 255)); - } + m_sdf_mapping_setting_->FromYamlFile(m_params_.sdf_mapping_config_path); + + // m_surface_mapping_ = std::make_shared(m_surface_mapping_setting_); + m_surface_mapping_setting_ = m_sdf_mapping_setting_->surface_mapping; + m_surface_mapping_setting_->FromYamlFile(m_params_.surface_mapping_config_path); + std::shared_ptr lidar_frame_setting = m_surface_mapping_setting_->sensor_gp->lidar_frame; + lidar_frame_setting->angle_min = -3.14; + lidar_frame_setting->angle_max = 3.14; + lidar_frame_setting->num_rays = 1024; + m_sdf_mapping_ = std::make_shared(m_sdf_mapping_setting_); + m_surface_mapping_ = m_sdf_mapping_->GetSurfaceMapping(); + + std::cout << lidar_frame_setting->angle_min << "\n"; + std::cout << lidar_frame_setting->angle_max << "\n"; + std::cout << lidar_frame_setting->num_rays << "\n"; + m_spinner_->start(); m_lidar_scan_sub_ = std::make_shared( @@ -170,17 +151,12 @@ class RosNode { LOAD_PARAMETER(lidar_frame_name); LOAD_PARAMETER(surface_mapping_config_path); LOAD_PARAMETER(sdf_mapping_config_path); - m_surface_mapping_setting_ = std::make_shared(); - if (!m_params_.surface_mapping_config_path.empty()) { m_surface_mapping_setting_->FromYamlFile(m_params_.surface_mapping_config_path); } + // m_surface_mapping_setting_ = std::make_shared(); + m_sdf_mapping_setting_ = std::make_shared(); - if (!m_params_.sdf_mapping_config_path.empty()) { m_sdf_mapping_setting_->FromYamlFile(m_params_.sdf_mapping_config_path); } + LOAD_PARAMETER(visualize_quadtree); - if (m_params_.visualize_quadtree) { - LOAD_PARAMETER(visualize_frequency_divider); - LOAD_REQUIRED_PARAMETER(visualize_quadtree_config_path); - m_quadtree_drawer_setting_ = std::make_shared(); - m_quadtree_drawer_setting_->FromYamlFile(m_params_.visualize_quadtree_config_path); - } + } #undef LOAD_REQUIRED_PARAMETER @@ -205,12 +181,30 @@ class RosNode { void SubCallbackLidarScan(const sensor_msgs::LaserScanConstPtr &laser_scan) { + if (m_surface_mapping_setting_ == nullptr){ + std::cout << "========================================\n"; + std::cout << "m_surface_mapping_setting_ is a NULL PTR\n"; + std::cout << "========================================\n"; + } + if (m_sdf_mapping_setting_->surface_mapping == nullptr){ + std::cout << "========================================\n"; + std::cout << "m_sdf_mapping_setting_->surface_mapping is a NULL PTR\n"; + std::cout << "========================================\n"; + } + if (m_sdf_mapping_->GetSetting()->surface_mapping == nullptr){ + std::cout << "========================================\n"; + std::cout << "m_sdf_mapping_->GetSetting()->surface_mapping is a NULL PTR\n"; + std::cout << "========================================\n"; + } + auto t0 = std::chrono::high_resolution_clock::now(); auto num_lines = static_cast(laser_scan->ranges.size()); Eigen::VectorXd angles = Eigen::VectorXd::LinSpaced(num_lines, laser_scan->angle_min, laser_scan->angle_max); Eigen::VectorXd ranges = Eigen::Map((const float *) (laser_scan->ranges.data()), num_lines).cast(); auto lidar_pose = GetLidarPose(laser_scan->header.stamp); - bool success = m_sdf_mapping_->Update(angles, ranges, lidar_pose); + Eigen::Matrix2d rotation = lidar_pose.leftCols<2>(); + Eigen::Vector2d translation = lidar_pose.rightCols<1>(); + bool success = m_sdf_mapping_->Update(rotation, translation, ranges); if (!success) { ROS_WARN("SDF mapping update failed"); return;