fix(interpolation): fix spline interpolation when the number of input…

… data is 3 (tier4#1162) * fix(interpolation): fix spline interpolation when the num of input data is 3 Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * test(interpolation): add a random query test when size of base_keys is 3 Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com> * chore(interpolation): fix spell Signed-off-by: Makoto Kurihara <mkuri8m@gmail.com>
boyali · Oct 19, 2022 · b319987 · b319987
1 parent b2fc25e
commit b319987
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 2 deletions.
diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/interpolation/src/spline_interpolation.cpp
@@ -72,7 +72,7 @@ inline std::vector<double> solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma
       x[j] = p[j] * x[j + 1] + q[j];
     }
   } else {
-    x.push_back(d[0] / b[0]);
+    x[0] = (d[0] / b[0]);
   }
 
   return x;

diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp
@@ -61,7 +61,7 @@ TEST(spline_interpolation, slerp)
     }
   }
 
-  {  // curve: query_keys is same as random
+  {  // curve: query_keys is 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};
@@ -96,6 +96,18 @@ TEST(spline_interpolation, slerp)
       EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
     }
   }
+
+  {  // curve: query_keys is random. size of base_keys is 3 (edge case in the implementation)
+    const std::vector<double> base_keys{-1.5, 1.0, 5.0};
+    const std::vector<double> base_values{-1.2, 0.5, 1.0};
+    const std::vector<double> query_keys{-1.0, 0.0, 4.0};
+    const std::vector<double> ans{-0.808769, -0.077539, 1.035096};
+
+    const auto query_values = interpolation::slerp(base_keys, base_values, query_keys);
+    for (size_t i = 0; i < query_values.size(); ++i) {
+      EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon);
+    }
+  }
 }
 
 TEST(spline_interpolation, slerpYawFromPoints)