Fixed sample pcd

Signed-off-by: Shintaro Sakoda <shintaro.sakoda@tier4.jp>

Author: SakodaShintaro

localization/ndt_scan_matcher/test/stub_pcd_loader.hpp

@@ -46,13 +46,25 @@ class StubPcdLoader : public rclcpp::Node
   {
     (void)req;
     autoware_map_msgs::msg::PointCloudMapCellWithID pcd_map_cell_with_id;
-    constexpr float interval = 0.2f;
-    pcd_map_cell_with_id.metadata.min_x = -10;
-    pcd_map_cell_with_id.metadata.min_y = -10;
-    pcd_map_cell_with_id.metadata.max_x = 30;
-    pcd_map_cell_with_id.metadata.max_y = 30;
     pcd_map_cell_with_id.cell_id = "0";
-    pcl::PointCloud<pcl::PointXYZ> cloud = make_sample_pcd(-10, 30, interval);
+    pcl::PointCloud<pcl::PointXYZ> cloud = make_sample_half_cubic_pcd();
+    const float offset_x = 100.0f;
+    const float offset_y = 100.0f;
+    for (auto & point : cloud.points) {
+      point.x += offset_x;
+      point.y += offset_y;
+    }
+    pcd_map_cell_with_id.metadata.min_x = std::numeric_limits<float>::max();
+    pcd_map_cell_with_id.metadata.min_y = std::numeric_limits<float>::max();
+    pcd_map_cell_with_id.metadata.max_x = std::numeric_limits<float>::lowest();
+    pcd_map_cell_with_id.metadata.max_y = std::numeric_limits<float>::lowest();
+    for (const auto & point : cloud.points) {
+      pcd_map_cell_with_id.metadata.min_x = std::min(pcd_map_cell_with_id.metadata.min_x, point.x);
+      pcd_map_cell_with_id.metadata.min_y = std::min(pcd_map_cell_with_id.metadata.min_y, point.y);
+      pcd_map_cell_with_id.metadata.max_x = std::max(pcd_map_cell_with_id.metadata.max_x, point.x);
+      pcd_map_cell_with_id.metadata.max_y = std::max(pcd_map_cell_with_id.metadata.max_y, point.y);
+    }
+    RCLCPP_INFO_STREAM(get_logger(), "cloud size: " << cloud.size());
     pcl::toROSMsg(cloud, pcd_map_cell_with_id.pointcloud);
     res->new_pointcloud_with_ids.push_back(pcd_map_cell_with_id);
     res->header.frame_id = "map";

localization/ndt_scan_matcher/test/test.cpp

@@ -91,7 +91,12 @@ TEST_F(TestNDTScanMatcher,
   //---------//
 
   // prepare input source PointCloud
-  pcl::PointCloud<pcl::PointXYZ> cloud = make_sample_pcd(-10.0, 10.0, 1.0);
+  pcl::PointCloud<pcl::PointXYZ> cloud = make_sample_half_cubic_pcd();
+  pcl::VoxelGrid<pcl::PointXYZ> vg;
+  vg.setInputCloud(cloud.makeShared());
+  vg.setLeafSize(0.5, 0.5, 0.5);
+  vg.filter(cloud);
+  RCLCPP_INFO_STREAM(node_->get_logger(), "sensor cloud size: " << cloud.size());
   sensor_msgs::msg::PointCloud2 input_cloud;
   pcl::toROSMsg(cloud, input_cloud);
   input_cloud.header.frame_id = "sensor_frame";
@@ -100,8 +105,8 @@ TEST_F(TestNDTScanMatcher,
 
   // prepare input initial pose
   geometry_msgs::msg::PoseWithCovarianceStamped initial_pose_msg;
-  initial_pose_msg.pose.pose.position.x = 10.0;
-  initial_pose_msg.pose.pose.position.y = 10.0;
+  initial_pose_msg.pose.pose.position.x = 100.0;
+  initial_pose_msg.pose.pose.position.y = 100.0;
   initial_pose_msg.pose.pose.position.z = 0.0;
   initial_pose_msg.pose.pose.orientation.x = 0.0;
   initial_pose_msg.pose.pose.orientation.y = 0.0;
@@ -138,9 +143,9 @@ TEST_F(TestNDTScanMatcher,
   RCLCPP_INFO_STREAM(
     node_->get_logger(), std::fixed << "result_pose: " << result_pose.position.x << ", "
                                     << result_pose.position.y << ", " << result_pose.position.z);
-  EXPECT_NEAR(result_pose.position.x, 10.0, 3.0);
-  EXPECT_NEAR(result_pose.position.y, 10.0, 3.0);
-  EXPECT_NEAR(result_pose.position.z, 0.0, 3.0);
+  EXPECT_NEAR(result_pose.position.x, 100.0, 1.0);
+  EXPECT_NEAR(result_pose.position.y, 100.0, 1.0);
+  EXPECT_NEAR(result_pose.position.z, 0.0, 1.0);
 
   rclcpp::shutdown();
   t1.join();

localization/ndt_scan_matcher/test/test_util.hpp

@@ -18,24 +18,35 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
-inline pcl::PointCloud<pcl::PointXYZ> make_sample_pcd(
-  const float min_xy, const float max_xy, const float interval)
+inline pcl::PointCloud<pcl::PointXYZ> make_sample_half_cubic_pcd()
 {
-  const float range_width = max_xy - min_xy;
-  const float center = (max_xy + min_xy) / 2.0f;
-  const int num_points_per_line = static_cast<int>(range_width / interval) + 1;
+  constexpr float length = 10;
+  constexpr float interval = 0.1;
+  constexpr int num_points_per_line = static_cast<int>(length / interval) + 1;
+  constexpr int num_points_per_plane = num_points_per_line * num_points_per_line;
   pcl::PointCloud<pcl::PointXYZ> cloud;
-  cloud.width = num_points_per_line * num_points_per_line;
+  cloud.width = 3 * num_points_per_plane;
   cloud.height = 1;
   cloud.points.resize(cloud.width * cloud.height);
   for (int i = 0; i < num_points_per_line; ++i) {
     for (int j = 0; j < num_points_per_line; ++j) {
-      const float x = min_xy + interval * static_cast<float>(j);
-      const float y = min_xy + interval * static_cast<float>(i);
-      const float z = std::hypot(x - center, y - center) / (range_width / 16);
-      cloud.points[i * num_points_per_line + j].x = x;  // NOLINT
-      cloud.points[i * num_points_per_line + j].y = y;  // NOLINT
-      cloud.points[i * num_points_per_line + j].z = z;  // NOLINT
+      const float u = interval * static_cast<float>(j);
+      const float v = interval * static_cast<float>(i);
+
+      // xy
+      cloud.points[num_points_per_plane * 0 + i * num_points_per_line + j].x = u;  // NOLINT
+      cloud.points[num_points_per_plane * 0 + i * num_points_per_line + j].y = v;  // NOLINT
+      cloud.points[num_points_per_plane * 0 + i * num_points_per_line + j].z = 0;  // NOLINT
+
+      // yz
+      cloud.points[num_points_per_plane * 1 + i * num_points_per_line + j].x = 0;  // NOLINT
+      cloud.points[num_points_per_plane * 1 + i * num_points_per_line + j].y = u;  // NOLINT
+      cloud.points[num_points_per_plane * 1 + i * num_points_per_line + j].z = v;  // NOLINT
+
+      // zx
+      cloud.points[num_points_per_plane * 2 + i * num_points_per_line + j].x = u;  // NOLINT
+      cloud.points[num_points_per_plane * 2 + i * num_points_per_line + j].y = 0;  // NOLINT
+      cloud.points[num_points_per_plane * 2 + i * num_points_per_line + j].z = v;  // NOLINT
     }
   }
   return cloud;