Removed Visualization. Fixed function signatures

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

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

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<const Setting>
+        [[nodiscard]] std::shared_ptr<Setting>
         GetSetting() const {
             return m_setting_;
         }

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<Setting, GpSdfMappingBaseSetting> {
             std::string surface_mapping_type = "erl::sdf_mapping::GpOccSurfaceMapping2D";
             std::string surface_mapping_setting_type = "erl::sdf_mapping::GpOccSurfaceMapping2D::Setting";
-            std::shared_ptr<AbstractSurfaceMapping2D::Setting> surface_mapping = nullptr;
+            std::shared_ptr<GpOccSurfaceMapping2D::Setting> surface_mapping = std::make_shared<GpOccSurfaceMapping2D::Setting>();
         };
 
         inline static const volatile bool kSettingRegistered = common::YamlableBase::Register<Setting>();
@@ -59,7 +60,7 @@ namespace erl::sdf_mapping {
 
         std::shared_ptr<Setting> m_setting_ = std::make_shared<Setting>();
         std::mutex m_mutex_;
-        std::shared_ptr<AbstractSurfaceMapping2D> m_surface_mapping_ = nullptr;       // for getting surface points, racing condition.
+        std::shared_ptr<GpOccSurfaceMapping2D> 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<AbstractSurfaceMapping2D>
+        [[nodiscard]] std::shared_ptr<GpOccSurfaceMapping2D>
         GetSurfaceMapping() const {
             return m_surface_mapping_;
         }
@@ -204,7 +205,7 @@ struct YAML::convert<erl::sdf_mapping::GpSdfMapping2D::Setting> {
         if (!convert<erl::sdf_mapping::GpSdfMappingBaseSetting>::decode(node, rhs)) { return false; }
         rhs.surface_mapping_type = node["surface_mapping_type"].as<std::string>();
         rhs.surface_mapping_setting_type = node["surface_mapping_setting_type"].as<std::string>();
-        using SettingBase = erl::sdf_mapping::AbstractSurfaceMapping2D::Setting;
+        using SettingBase = erl::sdf_mapping::GpOccSurfaceMapping2D::Setting;
         rhs.surface_mapping = SettingBase::Create<SettingBase>(rhs.surface_mapping_setting_type);
         if (rhs.surface_mapping == nullptr) {
             ERL_WARN("Failed to decode surface_mapping of type: {}", rhs.surface_mapping_setting_type);

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<std::size_t>(-1)) { return false; }
-            ERL_DEBUG_ASSERT(dynamic_cast<const SurfaceMappingOctreeNode *>(other) != nullptr, "other node is not SurfaceMappingOctreeNode.");
+            ERL_DEBUG_ASSERT(dynamic_cast<const SurfaceMappingQuadtreeNode *>(other) != nullptr, "other node is not SurfaceMappingQuadtreeNode.");
             if (const auto *other_node = reinterpret_cast<const SurfaceMappingQuadtreeNode *>(other);
                 other_node->surface_data_index != static_cast<std::size_t>(-1)) {
                 return false;

launch/sdf_mapping.launch

@@ -0,0 +1,14 @@
+<launch>
+	<!-- Launch the erl_sdf_mapping_node -->
+	<node name="erl_sdf_mapping_node" pkg="erl_sdf_mapping" type="erl_sdf_mapping_node" output="screen">
+		<param name="frequency" type="double" value="1.0"/>
+		<param name="lidar_data_topic_path" type="string" value="/front/scan"/>
+		<param name="lidar_frame_name" type="string" value="os_lidar"/>
+		<param name="visualize_quadtree" type="bool" value="false"/>
+		<param name="surface_mapping_config_path" type="string" value="/home/shantanu/SDF-Planning/src/erl_sdf_mapping/config/surface_mapping_2d.yaml"/>
+		<!-- rosparam file="/home/shantanu/SDF-Planning/src/erl_sdf_mapping/config/sdf_mapping_ucsd_fah_2d.yaml" command="load" -->
+		<rosparam file="/home/shantanu/SDF-Planning/src/erl_sdf_mapping/config/surface_mapping_2d.yaml" command="load"/>
+		<param name="sdf_mapping_config_path" type="string" value="/home/shantanu/SDF-Planning/src/erl_sdf_mapping/config/sdf_mapping_ucsd_fah_2d.yaml"/>
+	</node>
+	<!-- node pkg="rosbag" type="play" name="rosbag_play" args="/home/shantanu/Downloads/long_succ4_till_no_path.bag" -->
+</launch>

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;

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;

src/gp_sdf_mapping_2d.cpp

@@ -50,8 +50,7 @@ namespace erl::sdf_mapping {
 
     GpSdfMapping2D::GpSdfMapping2D(std::shared_ptr<Setting> setting)
         : m_setting_(NotNull(std::move(setting), "setting is nullptr.")),
-          m_surface_mapping_(
-              AbstractSurfaceMapping::CreateSurfaceMapping<AbstractSurfaceMapping2D>(m_setting_->surface_mapping_type, m_setting_->surface_mapping)) {
+          m_surface_mapping_(std::make_shared<GpOccSurfaceMapping2D>(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<AbstractSurfaceMapping2D>(
+                            m_surface_mapping_ = AbstractSurfaceMapping::CreateSurfaceMapping<GpOccSurfaceMapping2D>(
                                 m_setting_->surface_mapping_type,
                                 m_setting_->surface_mapping);
                         }

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 <cv_bridge/cv_bridge.h>
 #include <erl_sdf_mapping/PredictSdf.h>
@@ -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<erl::sdf_mapping::GpOccSurfaceMapping2D>(m_surface_mapping_setting_);
-        m_sdf_mapping_ = std::make_shared<erl::sdf_mapping::GpSdfMapping2D>(m_surface_mapping_, m_sdf_mapping_setting_);
-        if (m_params_.visualize_quadtree) {
-            m_quadtree_drawer_ = std::make_shared<OccupancyQuadtreeDrawer>(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<erl::sdf_mapping::GpOccSurfaceMapping2D>(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<erl::geometry::LidarFrame2D::Setting> 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<erl::sdf_mapping::GpSdfMapping2D>(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<ros::Subscriber>(
@@ -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<erl::sdf_mapping::GpOccSurfaceMapping2D::Setting>();
-        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<erl::sdf_mapping::GpOccSurfaceMapping2D::Setting>();
+
         m_sdf_mapping_setting_ = std::make_shared<erl::sdf_mapping::GpSdfMapping2D::Setting>();
-        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<OccupancyQuadtreeDrawer::Setting>();
-            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<long>(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 Eigen::VectorXf>((const float *) (laser_scan->ranges.data()), num_lines).cast<double>();
         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;