Merge remote-tracking branch 'origin/awf-latest' into refactor/control_launch_xml

Author: mitukou1109

localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/README.md

@@ -46,7 +46,7 @@ ros2 launch autoware_launch ... \
 
 ### Rosbag
 
-#### [Sample rosbag and map (AWSIM data)](https://drive.google.com/file/d/1uMVwQQFcfs8JOqfoA1FqfH_fLPwQ71jK/view)
+#### [Sample rosbag and map (AWSIM data)](https://drive.google.com/file/d/1ZPsfDvOXFrMxtx7fb1W5sOXdAK1e71hY/view)
 
 This data is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/).
 Essentially, AR tag-based self-localization is not intended for such public road driving, but for driving in a smaller area, so the max driving speed is set at 15 km/h.
@@ -55,7 +55,7 @@ It is a known problem that the timing of when each AR tag begins to be detected
 
 ![sample_result_in_awsim](./doc_image/sample_result_in_awsim.png)
 
-#### [Sample rosbag and map (Real world data)](https://drive.google.com/file/d/1wiCQjyjRnYbb0dg8G6mRecdSGh8tv3zR/view)
+#### [Sample rosbag and map (Real world data)](https://drive.google.com/file/d/1VQCQ_qiEZpCMI3-z6SNs__zJ-4HJFQjx/view)
 
 Please remap the topic names and play it.
 

localization/pose_estimator_arbiter/README.md

@@ -45,7 +45,7 @@ The following video demonstrates the switching of four different pose estimators
 
 Users can reproduce the demonstration using the following data and launch command:
 
-[sample data (rosbag & map)](https://drive.google.com/file/d/1MxLo1Sw6PdvfkyOYf_9A5dZ9uli1vPvS/view)
+[sample data (rosbag & map)](https://drive.google.com/file/d/1ZNlkyCtwe04iKFREdeZ5xuMU_jWpwM3W/view)
 The rosbag is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/).
 The map is an edited version of the [original map data](https://github.com/tier4/AWSIM/releases/download/v1.1.0/nishishinjuku_autoware_map.zip) published on the AWSIM documentation page to make it suitable for multiple pose_estimators.
 

perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_detected_object_fusion/node.hpp

@@ -18,6 +18,8 @@
 #include "autoware/universe_utils/ros/debug_publisher.hpp"
 #include "image_projection_based_fusion/fusion_node.hpp"
 
+#include <image_projection_based_fusion/utils/utils.hpp>
+
 #include "autoware_perception_msgs/msg/object_classification.hpp"
 
 #include <map>
@@ -45,7 +47,8 @@ class RoiDetectedObjectFusionNode
 
   std::map<std::size_t, DetectedObjectWithFeature> generateDetectedObjectRoIs(
     const DetectedObjects & input_object_msg, const double image_width, const double image_height,
-    const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection);
+    const Eigen::Affine3d & object2camera_affine,
+    const image_geometry::PinholeCameraModel & pinhole_camera_model);
 
   void fuseObjectsOnImage(
     const DetectedObjects & input_object_msg,

perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp

@@ -17,6 +17,8 @@
 
 #include "image_projection_based_fusion/fusion_node.hpp"
 
+#include <image_projection_based_fusion/utils/utils.hpp>
+
 #include <string>
 #include <vector>
 namespace image_projection_based_fusion

perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp

@@ -17,6 +17,8 @@
 
 #include "image_projection_based_fusion/fusion_node.hpp"
 
+#include <image_projection_based_fusion/utils/utils.hpp>
+
 #include <string>
 #include <vector>
 

perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp

@@ -42,6 +42,7 @@
 #include "autoware_perception_msgs/msg/shape.hpp"
 #include "tier4_perception_msgs/msg/detected_object_with_feature.hpp"
 
+#include <image_geometry/pinhole_camera_model.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
@@ -60,6 +61,10 @@ struct PointData
   float distance;
   size_t orig_index;
 };
+
+Eigen::Vector2d calcRawImageProjectedPoint(
+  const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d);
+
 std::optional<geometry_msgs::msg::TransformStamped> getTransformStamped(
   const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id,
   const std::string & source_frame_id, const rclcpp::Time & time);

perception/image_projection_based_fusion/package.xml

@@ -21,6 +21,7 @@
   <depend>autoware_universe_utils</depend>
   <depend>cv_bridge</depend>
   <depend>euclidean_cluster</depend>
+  <depend>image_geometry</depend>
   <depend>image_transport</depend>
   <depend>lidar_centerpoint</depend>
   <depend>message_filters</depend>

perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -307,9 +307,9 @@ void PointPaintingFusionNode::fuseOnSingleImage(
   const auto class_offset = painted_pointcloud_msg.fields.at(4).offset;
   const auto p_step = painted_pointcloud_msg.point_step;
   // projection matrix
-  Eigen::Matrix3f camera_projection;  // use only x,y,z
-  camera_projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2),
-    camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6);
+  image_geometry::PinholeCameraModel pinhole_camera_model;
+  pinhole_camera_model.fromCameraInfo(camera_info);
+
   Eigen::Vector3f point_lidar, point_camera;
   /** dc : don't care
 
@@ -342,15 +342,15 @@ dc   | dc dc dc  dc ||zc|
       continue;
     }
     // project
-    Eigen::Vector3f normalized_projected_point = camera_projection * Eigen::Vector3f(p_x, p_y, p_z);
+    Eigen::Vector2d projected_point =
+      calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(p_x, p_y, p_z));
+
     // iterate 2d bbox
     for (const auto & feature_object : objects) {
       sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi;
       // paint current point if it is inside bbox
       int label2d = feature_object.object.classification.front().label;
-      if (
-        !isUnknown(label2d) &&
-        isInsideBbox(normalized_projected_point.x(), normalized_projected_point.y(), roi, p_z)) {
+      if (!isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, p_z)) {
         data = &painted_pointcloud_msg.data[0];
         auto p_class = reinterpret_cast<float *>(&output[stride + class_offset]);
         for (const auto & cls : isClassTable_) {
@@ -361,7 +361,7 @@ dc   | dc dc dc  dc ||zc|
 #if 0
       // Parallelizing loop don't support push_back
       if (debugger_) {
-        debug_image_points.push_back(normalized_projected_point);
+        debug_image_points.push_back(projected_point);
       }
 #endif
     }

perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp

@@ -83,10 +83,8 @@ void RoiClusterFusionNode::fuseOnSingleImage(
   const DetectedObjectsWithFeature & input_roi_msg,
   const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg)
 {
-  Eigen::Matrix4d projection;
-  projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3),
-    camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7),
-    camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11);
+  image_geometry::PinholeCameraModel pinhole_camera_model;
+  pinhole_camera_model.fromCameraInfo(camera_info);
 
   // get transform from cluster frame id to camera optical frame id
   geometry_msgs::msg::TransformStamped transform_stamped;
@@ -133,23 +131,19 @@ void RoiClusterFusionNode::fuseOnSingleImage(
         continue;
       }
 
-      Eigen::Vector4d projected_point =
-        projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0);
-      Eigen::Vector2d normalized_projected_point = Eigen::Vector2d(
-        projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z());
+      Eigen::Vector2d projected_point =
+        calcRawImageProjectedPoint(pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z));
       if (
-        0 <= static_cast<int>(normalized_projected_point.x()) &&
-        static_cast<int>(normalized_projected_point.x()) <=
-          static_cast<int>(camera_info.width) - 1 &&
-        0 <= static_cast<int>(normalized_projected_point.y()) &&
-        static_cast<int>(normalized_projected_point.y()) <=
-          static_cast<int>(camera_info.height) - 1) {
-        min_x = std::min(static_cast<int>(normalized_projected_point.x()), min_x);
-        min_y = std::min(static_cast<int>(normalized_projected_point.y()), min_y);
-        max_x = std::max(static_cast<int>(normalized_projected_point.x()), max_x);
-        max_y = std::max(static_cast<int>(normalized_projected_point.y()), max_y);
-        projected_points.push_back(normalized_projected_point);
-        if (debugger_) debugger_->obstacle_points_.push_back(normalized_projected_point);
+        0 <= static_cast<int>(projected_point.x()) &&
+        static_cast<int>(projected_point.x()) <= static_cast<int>(camera_info.width) - 1 &&
+        0 <= static_cast<int>(projected_point.y()) &&
+        static_cast<int>(projected_point.y()) <= static_cast<int>(camera_info.height) - 1) {
+        min_x = std::min(static_cast<int>(projected_point.x()), min_x);
+        min_y = std::min(static_cast<int>(projected_point.y()), min_y);
+        max_x = std::max(static_cast<int>(projected_point.x()), max_x);
+        max_y = std::max(static_cast<int>(projected_point.y()), max_y);
+        projected_points.push_back(projected_point);
+        if (debugger_) debugger_->obstacle_points_.push_back(projected_point);
       }
     }
     if (projected_points.empty()) {

perception/image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp

@@ -86,15 +86,12 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage(
     object2camera_affine = transformToEigen(transform_stamped_optional.value().transform);
   }
 
-  Eigen::Matrix4d camera_projection;
-  camera_projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2),
-    camera_info.p.at(3), camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6),
-    camera_info.p.at(7), camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10),
-    camera_info.p.at(11);
+  image_geometry::PinholeCameraModel pinhole_camera_model;
+  pinhole_camera_model.fromCameraInfo(camera_info);
 
   const auto object_roi_map = generateDetectedObjectRoIs(
     input_object_msg, static_cast<double>(camera_info.width),
-    static_cast<double>(camera_info.height), object2camera_affine, camera_projection);
+    static_cast<double>(camera_info.height), object2camera_affine, pinhole_camera_model);
   fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map);
 
   if (debugger_) {
@@ -109,7 +106,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage(
 std::map<std::size_t, DetectedObjectWithFeature>
 RoiDetectedObjectFusionNode::generateDetectedObjectRoIs(
   const DetectedObjects & input_object_msg, const double image_width, const double image_height,
-  const Eigen::Affine3d & object2camera_affine, const Eigen::Matrix4d & camera_projection)
+  const Eigen::Affine3d & object2camera_affine,
+  const image_geometry::PinholeCameraModel & pinhole_camera_model)
 {
   std::map<std::size_t, DetectedObjectWithFeature> object_roi_map;
   int64_t timestamp_nsec =
@@ -148,13 +146,8 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs(
         continue;
       }
 
-      Eigen::Vector2d proj_point;
-      {
-        Eigen::Vector4d proj_point_hom =
-          camera_projection * Eigen::Vector4d(point.x(), point.y(), point.z(), 1.0);
-        proj_point = Eigen::Vector2d(
-          proj_point_hom.x() / (proj_point_hom.z()), proj_point_hom.y() / (proj_point_hom.z()));
-      }
+      Eigen::Vector2d proj_point = calcRawImageProjectedPoint(
+        pinhole_camera_model, cv::Point3d(point.x(), point.y(), point.z()));
 
       min_x = std::min(proj_point.x(), min_x);
       min_y = std::min(proj_point.y(), min_y);

perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp

@@ -101,10 +101,9 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
   }
 
   // transform pointcloud to camera optical frame id
-  Eigen::Matrix4d projection;
-  projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3),
-    camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7),
-    camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11);
+  image_geometry::PinholeCameraModel pinhole_camera_model;
+  pinhole_camera_model.fromCameraInfo(camera_info);
+
   geometry_msgs::msg::TransformStamped transform_stamped;
   {
     const auto transform_stamped_optional = getTransformStamped(
@@ -143,10 +142,8 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
     if (transformed_z <= 0.0) {
       continue;
     }
-    Eigen::Vector4d projected_point =
-      projection * Eigen::Vector4d(transformed_x, transformed_y, transformed_z, 1.0);
-    Eigen::Vector2d normalized_projected_point = Eigen::Vector2d(
-      projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z());
+    Eigen::Vector2d projected_point = calcRawImageProjectedPoint(
+      pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z));
     for (std::size_t i = 0; i < output_objs.size(); ++i) {
       auto & feature_obj = output_objs.at(i);
       const auto & check_roi = feature_obj.feature.roi;
@@ -156,10 +153,9 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
         continue;
       }
       if (
-        check_roi.x_offset <= normalized_projected_point.x() &&
-        check_roi.y_offset <= normalized_projected_point.y() &&
-        check_roi.x_offset + check_roi.width >= normalized_projected_point.x() &&
-        check_roi.y_offset + check_roi.height >= normalized_projected_point.y()) {
+        check_roi.x_offset <= projected_point.x() && check_roi.y_offset <= projected_point.y() &&
+        check_roi.x_offset + check_roi.width >= projected_point.x() &&
+        check_roi.y_offset + check_roi.height >= projected_point.y()) {
         std::memcpy(
           &cluster.data[clusters_data_size.at(i)], &input_pointcloud_msg.data[offset], point_step);
         clusters_data_size.at(i) += point_step;

perception/image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp

@@ -65,11 +65,9 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
   if (mask.cols == 0 || mask.rows == 0) {
     return;
   }
-  Eigen::Matrix4d projection;
-  projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3),
-    camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7),
-    camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11), 0.0, 0.0,
-    0.0, 1.0;
+  image_geometry::PinholeCameraModel pinhole_camera_model;
+  pinhole_camera_model.fromCameraInfo(camera_info);
+
   geometry_msgs::msg::TransformStamped transform_stamped;
   // transform pointcloud from frame id to camera optical frame id
   {
@@ -113,14 +111,11 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
       continue;
     }
 
-    Eigen::Vector4d projected_point =
-      projection * Eigen::Vector4d(transformed_x, transformed_y, transformed_z, 1.0);
-    Eigen::Vector2d normalized_projected_point = Eigen::Vector2d(
-      projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z());
+    Eigen::Vector2d projected_point = calcRawImageProjectedPoint(
+      pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z));
 
-    bool is_inside_image =
-      normalized_projected_point.x() > 0 && normalized_projected_point.x() < camera_info.width &&
-      normalized_projected_point.y() > 0 && normalized_projected_point.y() < camera_info.height;
+    bool is_inside_image = projected_point.x() > 0 && projected_point.x() < camera_info.width &&
+                           projected_point.y() > 0 && projected_point.y() < camera_info.height;
     if (!is_inside_image) {
       copyPointCloud(
         input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size);
@@ -129,8 +124,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
 
     // skip filtering pointcloud where semantic id out of the defined list
     uint8_t semantic_id = mask.at<uint8_t>(
-      static_cast<uint16_t>(normalized_projected_point.y()),
-      static_cast<uint16_t>(normalized_projected_point.x()));
+      static_cast<uint16_t>(projected_point.y()), static_cast<uint16_t>(projected_point.x()));
     if (static_cast<size_t>(semantic_id) >= filter_semantic_label_target_.size()) {
       copyPointCloud(
         input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size);

perception/image_projection_based_fusion/src/utils/utils.cpp

@@ -13,8 +13,18 @@
 // limitations under the License.
 
 #include "image_projection_based_fusion/utils/utils.hpp"
+
 namespace image_projection_based_fusion
 {
+Eigen::Vector2d calcRawImageProjectedPoint(
+  const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d)
+{
+  const cv::Point2d rectified_image_point = pinhole_camera_model.project3dToPixel(point3d);
+
+  const cv::Point2d raw_image_point = pinhole_camera_model.unrectifyPoint(rectified_image_point);
+
+  return Eigen::Vector2d(raw_image_point.x, raw_image_point.y);
+}
 
 std::optional<geometry_msgs::msg::TransformStamped> getTransformStamped(
   const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id,

perception/object_range_splitter/CMakeLists.txt

@@ -4,12 +4,12 @@ project(object_range_splitter)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-ament_auto_add_library(object_range_splitter SHARED
-  src/node.cpp
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/object_range_splitter_node.cpp
 )
 
-rclcpp_components_register_node(object_range_splitter
-  PLUGIN "object_range_splitter::ObjectRangeSplitterNode"
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::object_range_splitter::ObjectRangeSplitterNode"
   EXECUTABLE object_range_splitter_node
 )
 

perception/object_range_splitter/package.xml

@@ -14,7 +14,6 @@
   <depend>autoware_perception_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
-  <depend>sensor_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

perception/object_range_splitter/src/node.cpp perception/object_range_splitter/src/object_range_splitter_node.cpp

@@ -12,9 +12,9 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "object_range_splitter/node.hpp"
+#include "object_range_splitter_node.hpp"
 
-namespace object_range_splitter
+namespace autoware::object_range_splitter
 {
 ObjectRangeSplitterNode::ObjectRangeSplitterNode(const rclcpp::NodeOptions & node_options)
 : Node("object_range_splitter_node", node_options)
@@ -59,7 +59,7 @@ void ObjectRangeSplitterNode::objectCallback(
   long_range_object_pub_->publish(output_long_range_object_msg);
   short_range_object_pub_->publish(output_short_range_object_msg);
 }
-}  // namespace object_range_splitter
+}  // namespace autoware::object_range_splitter
 
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(object_range_splitter::ObjectRangeSplitterNode)
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::object_range_splitter::ObjectRangeSplitterNode)