perf(distortion_corrector_node): performance tuning

common/tier4_autoware_utils/include/tier4_autoware_utils/math/precompute.hpp

@@ -0,0 +1,61 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TIER4_AUTOWARE_UTILS__MATH__PRECOMPUTE_HPP_
+#define TIER4_AUTOWARE_UTILS__MATH__PRECOMPUTE_HPP_
+
+#include <cmath>
+#include <vector>
+
+namespace tier4_autoware_utils
+{
+
+class PrecomputedTrigFunc
+{
+  const float PI = std::acos(-1);
+
+  size_t table_size_;
+  std::vector<float> sin_table_;
+  std::vector<float> cos_table_;
+
+public:
+  PrecomputedTrigFunc(size_t table_size) : table_size_(table_size)
+  {
+    sin_table_.resize(table_size_);
+    cos_table_.resize(table_size_);
+
+    for (size_t i = 0; i < table_size_; i++) {
+      float degree = 360.f * i / table_size_;
+      float radian = degree * (PI / 180.f);
+      sin_table_[i] = std::sin(radian);
+      cos_table_[i] = std::cos(radian);
+    }
+  }
+
+  float sin(float radian)
+  {
+    float degree = radian * (180.f / PI) * (table_size_ / 360.f);
+    return sin_table_[(std::lround(degree) % table_size_ + table_size_) % table_size_];
+  }
+
+  inline float cos(float radian)
+  {
+    float degree = radian * (180.f / PI) * (table_size_ / 360.f);
+    return cos_table_[(std::lround(degree) % table_size_ + table_size_) % table_size_];
+  }
+};
+
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__MATH__PRECOMPUTE_HPP_

common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp

@@ -22,6 +22,7 @@
 #include "tier4_autoware_utils/geometry/pose_deviation.hpp"
 #include "tier4_autoware_utils/math/constants.hpp"
 #include "tier4_autoware_utils/math/normalization.hpp"
+#include "tier4_autoware_utils/math/precompute.hpp"
 #include "tier4_autoware_utils/math/range.hpp"
 #include "tier4_autoware_utils/math/unit_conversion.hpp"
 #include "tier4_autoware_utils/ros/debug_publisher.hpp"

sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp

@@ -36,6 +36,7 @@
 #include <tf2_ros/transform_listener.h>
 
 // Include tier4 autoware utils
+#include <tier4_autoware_utils/math/precompute.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
 
@@ -48,6 +49,8 @@ namespace pointcloud_preprocessor
 using rcl_interfaces::msg::SetParametersResult;
 using sensor_msgs::msg::PointCloud2;
 
+static const int SINCOS_TABLE_SIZE = 36000;
+
 class DistortionCorrectorComponent : public rclcpp::Node
 {
 public:
@@ -81,6 +84,8 @@ class DistortionCorrectorComponent : public rclcpp::Node
   std::string base_link_frame_ = "base_link";
   std::string time_stamp_field_name_;
   bool use_imu_;
+
+  std::shared_ptr<tier4_autoware_utils::PrecomputedTrigFunc> trig_func_;
 };
 
 }  // namespace pointcloud_preprocessor

sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

@@ -14,6 +14,7 @@
 
 #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp"
 
+#include <cmath>
 #include <deque>
 #include <string>
 #include <utility>
@@ -24,6 +25,8 @@ namespace pointcloud_preprocessor
 DistortionCorrectorComponent::DistortionCorrectorComponent(const rclcpp::NodeOptions & options)
 : Node("distortion_corrector_node", options)
 {
+  trig_func_ = std::make_shared<tier4_autoware_utils::PrecomputedTrigFunc>(SINCOS_TABLE_SIZE);
+
   // initialize debug tool
   {
     using tier4_autoware_utils::DebugPublisher;
@@ -217,17 +220,30 @@ bool DistortionCorrectorComponent::undistortPointCloud(
   }
 
   const tf2::Transform tf2_base_link_to_sensor_inv{tf2_base_link_to_sensor.inverse()};
+
+  // For performance, do not instantiate `rclcpp::Time` inside of the for-loop
+  double twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds();
+  double imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds();
+
+  // For performance, instantiate outside of the for-loop
+  tf2::Quaternion baselink_quat{};
+  tf2::Transform baselinkTF_odom{};
+  tf2::Vector3 point{};
+  tf2::Vector3 undistorted_point{};
+
+  // For performance, avoid transform computation if unnecessary
+  bool need_transform = points.header.frame_id != base_link_frame_;
+
   for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) {
-    for (;
-         (twist_it != std::end(twist_queue_) - 1 &&
-          *it_time_stamp > rclcpp::Time(twist_it->header.stamp).seconds());
-         ++twist_it) {
+    while (twist_it != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) {
+      ++twist_it;
+      twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds();
     }
 
     float v{static_cast<float>(twist_it->twist.linear.x)};
     float w{static_cast<float>(twist_it->twist.angular.z)};
 
-    if (std::abs(*it_time_stamp - rclcpp::Time(twist_it->header.stamp).seconds()) > 0.1) {
+    if (std::abs(*it_time_stamp - twist_stamp > 0.1)) {
       RCLCPP_WARN_STREAM_THROTTLE(
         get_logger(), *get_clock(), 10000 /* ms */,
         "twist time_stamp is too late. Could not interpolate.");
@@ -241,7 +257,13 @@ bool DistortionCorrectorComponent::undistortPointCloud(
             *it_time_stamp > rclcpp::Time(imu_it->header.stamp).seconds());
            ++imu_it) {
       }
-      if (std::abs(*it_time_stamp - rclcpp::Time(imu_it->header.stamp).seconds()) > 0.1) {
+
+      while (imu_it != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > imu_stamp) {
+        ++imu_it;
+        imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds();
+      }
+
+      if (std::abs(*it_time_stamp - imu_stamp > 0.1)) {
         RCLCPP_WARN_STREAM_THROTTLE(
           get_logger(), *get_clock(), 10000 /* ms */,
           "imu time_stamp is too late. Could not interpolate.");
@@ -252,28 +274,32 @@ bool DistortionCorrectorComponent::undistortPointCloud(
 
     const float time_offset = static_cast<float>(*it_time_stamp - prev_time_stamp_sec);
 
-    const tf2::Vector3 sensorTF_point{*it_x, *it_y, *it_z};
+    point.setValue(*it_x, *it_y, *it_z);
 
-    const tf2::Vector3 base_linkTF_point{tf2_base_link_to_sensor_inv * sensorTF_point};
+    if (need_transform) {
+      point = tf2_base_link_to_sensor_inv * point;
+    }
 
     theta += w * time_offset;
-    tf2::Quaternion baselink_quat{};
-    baselink_quat.setRPY(0.0, 0.0, theta);
+    baselink_quat.setValue(
+      0, 0, trig_func_->sin(theta * 0.5f),
+      trig_func_->cos(theta * 0.5f));  // baselink_quat.setRPY(0.0, 0.0, theta);
     const float dis = v * time_offset;
-    x += dis * std::cos(theta);
-    y += dis * std::sin(theta);
+    x += dis * trig_func_->cos(theta);
+    y += dis * trig_func_->sin(theta);
 
-    tf2::Transform baselinkTF_odom{};
     baselinkTF_odom.setOrigin(tf2::Vector3(x, y, 0.0));
     baselinkTF_odom.setRotation(baselink_quat);
 
-    const tf2::Vector3 base_linkTF_trans_point{baselinkTF_odom * base_linkTF_point};
+    undistorted_point = baselinkTF_odom * point;
 
-    const tf2::Vector3 sensorTF_trans_point{tf2_base_link_to_sensor * base_linkTF_trans_point};
+    if (need_transform) {
+      undistorted_point = tf2_base_link_to_sensor * undistorted_point;
+    }
 
-    *it_x = sensorTF_trans_point.getX();
-    *it_y = sensorTF_trans_point.getY();
-    *it_z = sensorTF_trans_point.getZ();
+    *it_x = undistorted_point.getX();
+    *it_y = undistorted_point.getY();
+    *it_z = undistorted_point.getZ();
 
     prev_time_stamp_sec = *it_time_stamp;
   }