style(pre-commit): autofix

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/camera_projection.hpp

@@ -18,13 +18,11 @@
 #define EIGEN_MPL2_ONLY
 
 #include <Eigen/Core>
-
 #include <autoware/universe_utils/system/lru_cache.hpp>
+#include <opencv2/core/core.hpp>
 
 #include <sensor_msgs/msg/camera_info.hpp>
 
-#include <opencv2/core/core.hpp>
-
 #include <image_geometry/pinhole_camera_model.h>
 
 #include <map>
@@ -42,22 +40,21 @@ class CameraProjection
 {
 public:
   explicit CameraProjection(
-    const sensor_msgs::msg::CameraInfo & camera_info,
-    const float grid_width, const float grid_height,
-    const bool unrectify,
-    const bool use_approximation
-  );
-  CameraProjection(): grid_w_size_(1.0), grid_h_size_(1.0), unrectify_(false) {};
+    const sensor_msgs::msg::CameraInfo & camera_info, const float grid_width,
+    const float grid_height, const bool unrectify, const bool use_approximation);
+  CameraProjection() : grid_w_size_(1.0), grid_h_size_(1.0), unrectify_(false) {};
   void initialize();
   std::function<bool(const cv::Point3d &, Eigen::Vector2d &)> calcImageProjectedPoint;
   sensor_msgs::msg::CameraInfo getCameraInfo();
   bool isOutsideHorizontalView(const float px, const float pz);
   bool isOutsideVerticalView(const float py, const float pz);
 
 protected:
-  bool calcRectifiedImageProjectedPoint(const cv::Point3d & point3d, Eigen::Vector2d & projected_point);
+  bool calcRectifiedImageProjectedPoint(
+    const cv::Point3d & point3d, Eigen::Vector2d & projected_point);
   bool calcRawImageProjectedPoint(const cv::Point3d & point3d, Eigen::Vector2d & projected_point);
-  bool calcRawImageProjectedPointWithApproximation(const cv::Point3d & point3d, Eigen::Vector2d & projected_point);
+  bool calcRawImageProjectedPointWithApproximation(
+    const cv::Point3d & point3d, Eigen::Vector2d & projected_point);
   void initializeCache();
 
   sensor_msgs::msg::CameraInfo camera_info_;

perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp

@@ -56,8 +56,8 @@ using sensor_msgs::msg::PointCloud2;
 using tier4_perception_msgs::msg::DetectedObjectsWithFeature;
 using tier4_perception_msgs::msg::DetectedObjectWithFeature;
 using PointCloud = pcl::PointCloud<pcl::PointXYZ>;
-using autoware_perception_msgs::msg::ObjectClassification;
 using autoware::image_projection_based_fusion::CameraProjection;
+using autoware_perception_msgs::msg::ObjectClassification;
 
 template <class TargetMsg3D, class ObjType, class Msg2D>
 class FusionNode : public rclcpp::Node

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

@@ -42,8 +42,7 @@ class RoiDetectedObjectFusionNode
 
   void fuseOnSingleImage(
     const DetectedObjects & input_object_msg, const std::size_t image_id,
-    const DetectedObjectsWithFeature & input_roi_msg,
-    DetectedObjects & output_object_msg) override;
+    const DetectedObjectsWithFeature & input_roi_msg, DetectedObjects & output_object_msg) override;
 
   std::map<std::size_t, DetectedObjectWithFeature> generateDetectedObjectRoIs(
     const DetectedObjects & input_object_msg, const std::size_t & image_id,

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

@@ -49,8 +49,7 @@ class RoiPointCloudFusionNode
 
   void fuseOnSingleImage(
     const PointCloud2 & input_pointcloud_msg, const std::size_t image_id,
-    const DetectedObjectsWithFeature & input_roi_msg,
-    PointCloud2 & output_pointcloud_msg) override;
+    const DetectedObjectsWithFeature & input_roi_msg, PointCloud2 & output_pointcloud_msg) override;
   bool out_of_scope(const DetectedObjectWithFeature & obj) override;
 };
 

perception/autoware_image_projection_based_fusion/src/camera_projection.cpp

@@ -29,8 +29,13 @@ namespace autoware::image_projection_based_fusion
 {
 CameraProjection::CameraProjection(
   const sensor_msgs::msg::CameraInfo & camera_info, const float grid_width, const float grid_height,
-  const bool unrectify, const bool use_approximation=false
-): camera_info_(camera_info), grid_w_size_(grid_width), grid_h_size_(grid_height), unrectify_(unrectify), use_approximation_(use_approximation){
+  const bool unrectify, const bool use_approximation = false)
+: camera_info_(camera_info),
+  grid_w_size_(grid_width),
+  grid_h_size_(grid_height),
+  unrectify_(unrectify),
+  use_approximation_(use_approximation)
+{
   if (grid_w_size_ <= 0.0 || grid_h_size_ <= 0.0) {
     throw std::runtime_error("Both grid_width and grid_height must be > 0.0");
   }
@@ -45,43 +50,48 @@ CameraProjection::CameraProjection(
   // for shifting to the grid center
   half_grid_w_size_ = grid_w_size_ / 2.0;
   half_grid_h_size_ = grid_h_size_ / 2.0;
-  inv_grid_w_size_ = 1/grid_w_size_;
-  inv_grid_h_size_ = 1/grid_h_size_;
+  inv_grid_w_size_ = 1 / grid_w_size_;
+  inv_grid_h_size_ = 1 / grid_h_size_;
   grid_x_num_ = static_cast<uint32_t>(std::ceil(image_w_ / grid_w_size_));
   grid_y_num_ = static_cast<uint32_t>(std::ceil(image_h_ / grid_h_size_));
-  cache_size_ = grid_x_num_*grid_y_num_;
+  cache_size_ = grid_x_num_ * grid_y_num_;
 
   // for checking the views
   // cx/fx
-  tan_h_x_ = camera_info.k.at(2)/camera_info.k.at(0);
+  tan_h_x_ = camera_info.k.at(2) / camera_info.k.at(0);
   // cy/fy
-  tan_h_y_ = camera_info.k.at(5)/camera_info.k.at(4);
+  tan_h_y_ = camera_info.k.at(5) / camera_info.k.at(4);
 }
 
-void CameraProjection::initialize(){
+void CameraProjection::initialize()
+{
   if (unrectify_) {
     if (use_approximation_) {
       // create the cache with size of grid center
       // store only xy position in float to reduce memory consumption
       projection_cache_ = std::make_unique<PixelPos[]>(cache_size_);
       initializeCache();
 
-      calcImageProjectedPoint = [this](const cv::Point3d & point3d, Eigen::Vector2d & projected_point) {
+      calcImageProjectedPoint = [this](
+                                  const cv::Point3d & point3d, Eigen::Vector2d & projected_point) {
         return this->calcRawImageProjectedPointWithApproximation(point3d, projected_point);
       };
     } else {
-      calcImageProjectedPoint = [this](const cv::Point3d & point3d, Eigen::Vector2d & projected_point) {
+      calcImageProjectedPoint = [this](
+                                  const cv::Point3d & point3d, Eigen::Vector2d & projected_point) {
         return this->calcRawImageProjectedPoint(point3d, projected_point);
       };
     }
   } else {
-    calcImageProjectedPoint = [this](const cv::Point3d & point3d, Eigen::Vector2d & projected_point) {
+    calcImageProjectedPoint = [this](
+                                const cv::Point3d & point3d, Eigen::Vector2d & projected_point) {
       return this->calcRectifiedImageProjectedPoint(point3d, projected_point);
     };
   }
 }
 
-void CameraProjection::initializeCache(){
+void CameraProjection::initializeCache()
+{
   // sample grid centers till the camera height, width to precompute the projection
   //
   //      grid_size
@@ -101,31 +111,35 @@ void CameraProjection::initializeCache(){
 
   for (float y = half_grid_h_size_; y < image_h_; y += grid_h_size_) {
     for (float x = half_grid_w_size_; x < image_w_; x += grid_w_size_) {
-      const float qx = std::round((x-half_grid_w_size_)*inv_grid_w_size_)*grid_w_size_+half_grid_w_size_;
-      const float qy = std::round((y-half_grid_h_size_)*inv_grid_h_size_)*grid_h_size_+half_grid_h_size_;
+      const float qx =
+        std::round((x - half_grid_w_size_) * inv_grid_w_size_) * grid_w_size_ + half_grid_w_size_;
+      const float qy =
+        std::round((y - half_grid_h_size_) * inv_grid_h_size_) * grid_h_size_ + half_grid_h_size_;
 
       const int grid_x = static_cast<int>(std::floor(qx / grid_w_size_));
       const int grid_y = static_cast<int>(std::floor(qy / grid_h_size_));
-      const int index = (grid_y) * grid_x_num_ + grid_x;
+      const int index = (grid_y)*grid_x_num_ + grid_x;
 
       // precompute projected point
       cv::Point2d raw_image_point = camera_model_.unrectifyPoint(cv::Point2d(qx, qy));
-      projection_cache_[index] = PixelPos{static_cast<float>(raw_image_point.x), static_cast<float>(raw_image_point.y)};
+      projection_cache_[index] =
+        PixelPos{static_cast<float>(raw_image_point.x), static_cast<float>(raw_image_point.y)};
     }
   }
 }
 
-
 /**
  * @brief Calculate a projection of 3D point to rectified image plane 2D point.
  * @return Return a boolean indicating whether the projected point is on the image plane.
  */
 bool CameraProjection::calcRectifiedImageProjectedPoint(
-  const cv::Point3d & point3d, Eigen::Vector2d & projected_point
-){
+  const cv::Point3d & point3d, Eigen::Vector2d & projected_point)
+{
   const cv::Point2d rectified_image_point = camera_model_.project3dToPixel(point3d);
 
-  if (rectified_image_point.x < 0.0 || rectified_image_point.x >= image_w_ || rectified_image_point.y < 0.0 || rectified_image_point.y >= image_h_){
+  if (
+    rectified_image_point.x < 0.0 || rectified_image_point.x >= image_w_ ||
+    rectified_image_point.y < 0.0 || rectified_image_point.y >= image_h_) {
     return false;
   } else {
     projected_point << rectified_image_point.x, rectified_image_point.y;
@@ -138,12 +152,14 @@ bool CameraProjection::calcRectifiedImageProjectedPoint(
  * @return Return a boolean indicating whether the projected point is on the image plane.
  */
 bool CameraProjection::calcRawImageProjectedPoint(
-  const cv::Point3d & point3d, Eigen::Vector2d & projected_point
-){
+  const cv::Point3d & point3d, Eigen::Vector2d & projected_point)
+{
   const cv::Point2d rectified_image_point = camera_model_.project3dToPixel(point3d);
   const cv::Point2d raw_image_point = camera_model_.unrectifyPoint(rectified_image_point);
 
-  if (rectified_image_point.x < 0.0 || rectified_image_point.x >= image_w_ || rectified_image_point.y < 0.0 || rectified_image_point.y >= image_h_){
+  if (
+    rectified_image_point.x < 0.0 || rectified_image_point.x >= image_w_ ||
+    rectified_image_point.y < 0.0 || rectified_image_point.y >= image_h_) {
     return false;
   } else {
     projected_point << raw_image_point.x, raw_image_point.y;
@@ -156,16 +172,20 @@ bool CameraProjection::calcRawImageProjectedPoint(
  * @return Return a boolean indicating whether the projected point is on the image plane.
  */
 bool CameraProjection::calcRawImageProjectedPointWithApproximation(
-  const cv::Point3d & point3d, Eigen::Vector2d & projected_point
-){
+  const cv::Point3d & point3d, Eigen::Vector2d & projected_point)
+{
   const cv::Point2d rectified_image_point = camera_model_.project3dToPixel(point3d);
 
   // round to a near grid center
-  const float qx = std::round((rectified_image_point.x-half_grid_w_size_)*inv_grid_w_size_)*grid_w_size_+half_grid_w_size_;
+  const float qx =
+    std::round((rectified_image_point.x - half_grid_w_size_) * inv_grid_w_size_) * grid_w_size_ +
+    half_grid_w_size_;
   if (qx < 0.0 || qx >= image_w_) {
     return false;
   }
-  const float qy = std::round((rectified_image_point.y-half_grid_h_size_)*inv_grid_h_size_)*grid_h_size_+half_grid_h_size_;
+  const float qy =
+    std::round((rectified_image_point.y - half_grid_h_size_) * inv_grid_h_size_) * grid_h_size_ +
+    half_grid_h_size_;
   if (qy < 0.0 || qy >= image_h_) {
     return false;
   }
@@ -179,16 +199,19 @@ bool CameraProjection::calcRawImageProjectedPointWithApproximation(
   return true;
 }
 
-sensor_msgs::msg::CameraInfo CameraProjection::getCameraInfo(){
+sensor_msgs::msg::CameraInfo CameraProjection::getCameraInfo()
+{
   return camera_info_;
 }
 
-bool CameraProjection::isOutsideHorizontalView(const float px, const float pz){
+bool CameraProjection::isOutsideHorizontalView(const float px, const float pz)
+{
   // assuming the points' origin is centered on the camera
   return pz <= 0.0 || px > tan_h_x_ * pz || px < -tan_h_x_ * pz;
 }
 
-bool CameraProjection::isOutsideVerticalView(const float py, const float pz){
+bool CameraProjection::isOutsideVerticalView(const float py, const float pz)
+{
   // assuming the points' origin is centered on the camera
   return pz <= 0.0 || py > tan_h_y_ * pz || py < -tan_h_y_ * pz;
 }

perception/autoware_image_projection_based_fusion/src/fusion_node.cpp

@@ -305,8 +305,7 @@ void FusionNode<TargetMsg3D, Obj, Msg2D>::subCallback(
         }
 
         fuseOnSingleImage(
-          *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]),
-          *output_msg);
+          *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]), *output_msg);
         (cached_roi_msgs_.at(roi_i)).erase(matched_stamp);
         is_fused_.at(roi_i) = true;
 

perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp

@@ -346,8 +346,7 @@ dc   | dc dc dc  dc ||zc|
         // paint current point if it is inside bbox
         int label2d = feature_object.object.classification.front().label;
         if (
-          !isUnknown(label2d) &&
-          isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) {
+          !isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) {
           // cppcheck-suppress invalidPointerCast
           auto p_class = reinterpret_cast<float *>(&output[stride + class_offset]);
           for (const auto & cls : isClassTable_) {

perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp

@@ -100,8 +100,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage(
     object2camera_affine = transformToEigen(transform_stamped_optional.value().transform);
   }
 
-  const auto object_roi_map = generateDetectedObjectRoIs(
-    input_object_msg, image_id, object2camera_affine);
+  const auto object_roi_map =
+    generateDetectedObjectRoIs(input_object_msg, image_id, object2camera_affine);
   fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map);
 
   if (debugger_) {
@@ -161,8 +161,7 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs(
 
       Eigen::Vector2d proj_point;
       if (camera_projectors_[image_id].calcImageProjectedPoint(
-        cv::Point3d(point.x(), point.y(), point.z()), proj_point
-      )){
+            cv::Point3d(point.x(), point.y(), point.z()), proj_point)) {
         const double px = proj_point.x();
         const double py = proj_point.y();
 

perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp

@@ -86,8 +86,7 @@ void RoiPointCloudFusionNode::postprocess(
   }
 }
 void RoiPointCloudFusionNode::fuseOnSingleImage(
-  const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg,
-  const std::size_t image_id,
+  const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id,
   const DetectedObjectsWithFeature & input_roi_msg,
   __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg)
 {
@@ -131,9 +130,12 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
     transform_stamped = transform_stamped_optional.value();
   }
   const int point_step = input_pointcloud_msg.point_step;
-  const int x_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "x")].offset;
-  const int y_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "y")].offset;
-  const int z_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "z")].offset;
+  const int x_offset =
+    input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "x")].offset;
+  const int y_offset =
+    input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "y")].offset;
+  const int z_offset =
+    input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "z")].offset;
 
   sensor_msgs::msg::PointCloud2 transformed_cloud;
   tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped);
@@ -161,8 +163,7 @@ void RoiPointCloudFusionNode::fuseOnSingleImage(
 
     Eigen::Vector2d projected_point;
     if (camera_projectors_[image_id].calcImageProjectedPoint(
-      cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)
-    ) {
+          cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) {
       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;

perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp

@@ -151,8 +151,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage(
 
     Eigen::Vector2d projected_point;
     if (!camera_projectors_[image_id].calcImageProjectedPoint(
-      cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point
-    )){
+          cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) {
       continue;
     }