feat(interpolation): add float key lerp api

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
autowarefoundation · Feb 18, 2022 · cd8e239 · cd8e239
1 parent 1278fe9
commit cd8e239
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 0 deletions.
diff --git a/common/interpolation/include/interpolation/linear_interpolation.hpp b/common/interpolation/include/interpolation/linear_interpolation.hpp
@@ -26,6 +26,11 @@ double lerp(const double src_val, const double dst_val, const double ratio);
 std::vector<double> lerp(
   const std::vector<double> & base_keys, const std::vector<double> & base_values,
   const std::vector<double> & query_keys);
+
+double lerp(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values,
+  const double query_key);
+
 }  // namespace interpolation
 
 #endif  // INTERPOLATION__LINEAR_INTERPOLATION_HPP_
diff --git a/common/interpolation/src/linear_interpolation.cpp b/common/interpolation/src/linear_interpolation.cpp
@@ -49,4 +49,10 @@ std::vector<double> lerp(
 
   return query_values;
 }
+
+double lerp(
+  const std::vector<double> & base_keys, const std::vector<double> & base_values, double query_key)
+{
+  return lerp(base_keys, base_values, std::vector<double>{query_key}).front();
+}
 }  // namespace interpolation
diff --git a/common/interpolation/test/src/test_linear_interpolation.cpp b/common/interpolation/test/src/test_linear_interpolation.cpp
@@ -77,3 +77,18 @@ TEST(linear_interpolation, lerp_vector)
     }
   }
 }
+
+TEST(linear_interpolation, lerp_scalar_query)
+{
+  {  // curve: query_keys is same as random
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0};
+    const std::vector<double> query_keys{0.0, 8.0, 18.0};
+    const std::vector<double> ans{-0.18, 1.12, 1.4};
+
+    for (size_t i = 0; i < query_keys.size(); ++i) {
+      const auto query_value = interpolation::lerp(base_keys, base_values, query_keys.at(i));
+      EXPECT_NEAR(query_value, ans.at(i), epsilon);
+    }
+  }
+}