perf(distortion_corrector_node): performance tuning (autowarefoundati…

…on#2913)

* Avoid unnecessary object instantiation

Signed-off-by: Takahiro Ishikawa <[email protected]>

* style(pre-commit): autofix

* Fix: Avoid unnecessary object instantiation

Signed-off-by: Takahiro Ishikawa <[email protected]>

* Minimize object instantiation

Signed-off-by: Takahiro Ishikawa <[email protected]>

* Avoid transform computation if possible

Signed-off-by: Takahiro Ishikawa <[email protected]>

* Pre-compute sin/cos

Signed-off-by: Takahiro Ishikawa <[email protected]>

* style(pre-commit): autofix

* Place sincos precompute under util

Signed-off-by: Takahiro Ishikawa <[email protected]>

* style(pre-commit): autofix

* Use hardcoded sin values for approcimation

Signed-off-by: Takahiro Ishikawa <[email protected]>

* style(pre-commit): autofix

* Use new approximated sin/cos functions

Signed-off-by: Takahiro Ishikawa <[email protected]>

* Improve accuracy of approximated sin/cos

Signed-off-by: Takahiro Ishikawa <[email protected]>

* style(pre-commit): autofix

* Add test for trigonometry

Signed-off-by: Takahiro Ishikawa <[email protected]>

* style(pre-commit): autofix

* Fix

Signed-off-by: Takahiro Ishikawa <[email protected]>

* Fix

Signed-off-by: Takahiro Ishikawa <[email protected]>

* Conform to clang-tidy

Signed-off-by: Takahiro Ishikawa <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: Takahiro Ishikawa <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

common/tier4_autoware_utils/CMakeLists.txt

@@ -9,6 +9,8 @@ find_package(Boost REQUIRED)
 ament_auto_add_library(tier4_autoware_utils SHARED
   src/tier4_autoware_utils.cpp
   src/geometry/boost_polygon_utils.cpp
+  src/math/sin_table.cpp
+  src/math/trigonometry.cpp
   src/ros/msg_operation.cpp
 )
 

common/tier4_autoware_utils/include/tier4_autoware_utils/math/sin_table.hpp

@@ -0,0 +1,30 @@
+// Copyright 2023 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__SIN_TABLE_HPP_
+#define TIER4_AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_
+
+#include <cstddef>
+
+namespace tier4_autoware_utils
+{
+
+constexpr size_t sin_table_size = 32769;
+constexpr size_t discrete_arcs_num_90 = 32768;
+constexpr size_t discrete_arcs_num_360 = 131072;
+extern const float g_sin_table[sin_table_size];
+
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__MATH__SIN_TABLE_HPP_

common/tier4_autoware_utils/include/tier4_autoware_utils/math/trigonometry.hpp

@@ -0,0 +1,27 @@
+// Copyright 2023 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__TRIGONOMETRY_HPP_
+#define TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_
+
+namespace tier4_autoware_utils
+{
+
+float sin(float radian);
+
+float cos(float radian);
+
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__MATH__TRIGONOMETRY_HPP_

common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp

@@ -23,6 +23,8 @@
 #include "tier4_autoware_utils/math/constants.hpp"
 #include "tier4_autoware_utils/math/normalization.hpp"
 #include "tier4_autoware_utils/math/range.hpp"
+#include "tier4_autoware_utils/math/sin_table.hpp"
+#include "tier4_autoware_utils/math/trigonometry.hpp"
 #include "tier4_autoware_utils/math/unit_conversion.hpp"
 #include "tier4_autoware_utils/ros/debug_publisher.hpp"
 #include "tier4_autoware_utils/ros/debug_traits.hpp"

common/tier4_autoware_utils/src/math/trigonometry.cpp

@@ -0,0 +1,49 @@
+// Copyright 2023 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.
+
+#include "tier4_autoware_utils/math/trigonometry.hpp"
+
+#include "tier4_autoware_utils/math/constants.hpp"
+#include "tier4_autoware_utils/math/sin_table.hpp"
+
+#include <cmath>
+
+namespace tier4_autoware_utils
+{
+
+float sin(float radian)
+{
+  float degree = radian * (180.f / static_cast<float>(tier4_autoware_utils::pi)) *
+                 (discrete_arcs_num_360 / 360.f);
+  size_t idx =
+    (static_cast<int>(std::round(degree)) % discrete_arcs_num_360 + discrete_arcs_num_360) %
+    discrete_arcs_num_360;
+
+  float mul = 1.f;
+  if (discrete_arcs_num_90 <= idx && idx < 2 * discrete_arcs_num_90) {
+    idx = 2 * discrete_arcs_num_90 - idx;
+  } else if (2 * discrete_arcs_num_90 <= idx && idx < 3 * discrete_arcs_num_90) {
+    mul = -1.f;
+    idx = idx - 2 * discrete_arcs_num_90;
+  } else if (3 * discrete_arcs_num_90 <= idx && idx < 4 * discrete_arcs_num_90) {
+    mul = -1.f;
+    idx = 4 * discrete_arcs_num_90 - idx;
+  }
+
+  return mul * g_sin_table[idx];
+}
+
+float cos(float radian) { return sin(radian + static_cast<float>(tier4_autoware_utils::pi) / 2.f); }
+
+}  // namespace tier4_autoware_utils

common/tier4_autoware_utils/test/src/math/test_trigonometry.cpp

@@ -0,0 +1,42 @@
+// Copyright 2023 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.
+
+#include "tier4_autoware_utils/math/constants.hpp"
+#include "tier4_autoware_utils/math/trigonometry.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+TEST(trigonometry, sin)
+{
+  float x = 4.f * tier4_autoware_utils::pi / 128.f;
+  for (int i = 0; i < 128; i++) {
+    EXPECT_TRUE(
+      std::abs(
+        std::sin(x * static_cast<float>(i)) -
+        tier4_autoware_utils::sin(x * static_cast<float>(i))) < 10e-5);
+  }
+}
+
+TEST(trigonometry, cos)
+{
+  float x = 4.f * tier4_autoware_utils::pi / 128.f;
+  for (int i = 0; i < 128; i++) {
+    EXPECT_TRUE(
+      std::abs(
+        std::cos(x * static_cast<float>(i)) -
+        tier4_autoware_utils::cos(x * static_cast<float>(i))) < 10e-5);
+  }
+}

sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

@@ -14,6 +14,8 @@
 
 #include "pointcloud_preprocessor/distortion_corrector/distortion_corrector.hpp"
 
+#include "tier4_autoware_utils/math/trigonometry.hpp"
+
 #include <deque>
 #include <string>
 #include <utility>
@@ -217,17 +219,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 baselink_tf_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 +256,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.");
@@ -250,30 +271,34 @@ bool DistortionCorrectorComponent::undistortPointCloud(
       }
     }
 
-    const float time_offset = static_cast<float>(*it_time_stamp - prev_time_stamp_sec);
+    const auto 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, tier4_autoware_utils::sin(theta * 0.5f),
+      tier4_autoware_utils::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 * tier4_autoware_utils::cos(theta);
+    y += dis * tier4_autoware_utils::sin(theta);
 
-    tf2::Transform baselinkTF_odom{};
-    baselinkTF_odom.setOrigin(tf2::Vector3(x, y, 0.0));
-    baselinkTF_odom.setRotation(baselink_quat);
+    baselink_tf_odom.setOrigin(tf2::Vector3(x, y, 0.0));
+    baselink_tf_odom.setRotation(baselink_quat);
 
-    const tf2::Vector3 base_linkTF_trans_point{baselinkTF_odom * base_linkTF_point};
+    undistorted_point = baselink_tf_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 = static_cast<float>(undistorted_point.getX());
+    *it_y = static_cast<float>(undistorted_point.getY());
+    *it_z = static_cast<float>(undistorted_point.getZ());
 
     prev_time_stamp_sec = *it_time_stamp;
   }