Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix(interpolation): use slerp for spherical_linear_interpolation #1769

Merged
merged 2 commits into from
Sep 2, 2022
Merged

fix(interpolation): use slerp for spherical_linear_interpolation #1769

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7de6c5e
fix(interpolation): use slerp for spherical_linear_interpolation
takayuki5168 Sep 2, 2022
5f58020
fix
takayuki5168 Sep 2, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion common/interpolation/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ Then it calculates interpolated values on y-axis for `query_keys` on x-axis.

## Spline Interpolation

`slerp(base_keys, base_values, query_keys)` (for vector interpolation) applies spline regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`.
`spline(base_keys, base_values, query_keys)` (for vector interpolation) applies spline regression to each two continuous points whose x values are`base_keys` and whose y values are `base_values`.
Then it calculates interpolated values on y-axis for `query_keys` on x-axis.

### Evaluation of calculation cost
Expand Down
4 changes: 2 additions & 2 deletions common/interpolation/include/interpolation/spherical_linear_interpolation.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,11 +31,11 @@

namespace interpolation
{
geometry_msgs::msg::Quaternion spherical_linear_interpolation(
geometry_msgs::msg::Quaternion slerp(
const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat,
const double ratio);

std::vector<geometry_msgs::msg::Quaternion> spherical_linear_interpolation(
std::vector<geometry_msgs::msg::Quaternion> slerp(
const std::vector<double> & base_keys,
const std::vector<geometry_msgs::msg::Quaternion> & base_values,
const std::vector<double> & query_keys);
Expand Down
4 changes: 2 additions & 2 deletions common/interpolation/include/interpolation/spline_interpolation.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,10 +46,10 @@ struct MultiSplineCoef
};

// static spline interpolation functions
std::vector<double> slerp(
std::vector<double> spline(
const std::vector<double> & base_keys, const std::vector<double> & base_values,
const std::vector<double> & query_keys);
std::vector<double> slerpByAkima(
std::vector<double> splineByAkima(
const std::vector<double> & base_keys, const std::vector<double> & base_values,
const std::vector<double> & query_keys);
} // namespace interpolation
Expand Down
6 changes: 3 additions & 3 deletions common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@
namespace interpolation
{
template <typename T>
std::vector<double> slerpYawFromPoints(const std::vector<T> & points);
std::vector<double> splineYawFromPoints(const std::vector<T> & points);
} // namespace interpolation

// non-static points spline interpolation
Expand Down Expand Up @@ -66,8 +66,8 @@ class SplineInterpolationPoints2d

private:
void calcSplineCoefficientsInner(const std::vector<geometry_msgs::msg::Point> & points);
SplineInterpolation slerp_x_;
SplineInterpolation slerp_y_;
SplineInterpolation spline_x_;
SplineInterpolation spline_y_;

std::vector<double> base_s_vec_;
};
Expand Down
6 changes: 3 additions & 3 deletions common/interpolation/src/spherical_linear_interpolation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@

namespace interpolation
{
geometry_msgs::msg::Quaternion spherical_linear_interpolation(
geometry_msgs::msg::Quaternion slerp(
const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat,
const double ratio)
{
Expand All @@ -28,7 +28,7 @@ geometry_msgs::msg::Quaternion spherical_linear_interpolation(
return tf2::toMsg(interpolated_quat);
}

std::vector<geometry_msgs::msg::Quaternion> spherical_linear_interpolation(
std::vector<geometry_msgs::msg::Quaternion> slerp(
const std::vector<double> & base_keys,
const std::vector<geometry_msgs::msg::Quaternion> & base_values,
const std::vector<double> & query_keys)
Expand All @@ -50,7 +50,7 @@ std::vector<geometry_msgs::msg::Quaternion> spherical_linear_interpolation(
const double ratio = (query_key - base_keys.at(key_index)) /
(base_keys.at(key_index + 1) - base_keys.at(key_index));

const auto interpolated_quat = spherical_linear_interpolation(src_quat, dst_quat, ratio);
const auto interpolated_quat = slerp(src_quat, dst_quat, ratio);
query_values.push_back(interpolated_quat);
}

Expand Down
4 changes: 2 additions & 2 deletions common/interpolation/src/spline_interpolation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ inline std::vector<double> solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma

namespace interpolation
{
std::vector<double> slerp(
std::vector<double> spline(
const std::vector<double> & base_keys, const std::vector<double> & base_values,
const std::vector<double> & query_keys)
{
Expand All @@ -94,7 +94,7 @@ std::vector<double> slerp(
return interpolator.getSplineInterpolatedValues(query_keys);
}

std::vector<double> slerpByAkima(
std::vector<double> splineByAkima(
const std::vector<double> & base_keys, const std::vector<double> & base_values,
const std::vector<double> & query_keys)
{
Expand Down
16 changes: 8 additions & 8 deletions common/interpolation/src/spline_interpolation_points_2d.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ std::array<std::vector<double>, 3> getBaseValues(
namespace interpolation
{
template <typename T>
std::vector<double> slerpYawFromPoints(const std::vector<T> & points)
std::vector<double> splineYawFromPoints(const std::vector<T> & points)
{
SplineInterpolationPoints2d interpolator;

Expand All @@ -84,7 +84,7 @@ std::vector<double> slerpYawFromPoints(const std::vector<T> & points)
}
return yaw_vec;
}
template std::vector<double> slerpYawFromPoints(
template std::vector<double> splineYawFromPoints(
const std::vector<geometry_msgs::msg::Point> & points);
} // namespace interpolation

Expand All @@ -103,8 +103,8 @@ geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoin
whole_s = base_s_vec_.back();
}

const double x = slerp_x_.getSplineInterpolatedValues({whole_s}).at(0);
const double y = slerp_y_.getSplineInterpolatedValues({whole_s}).at(0);
const double x = spline_x_.getSplineInterpolatedValues({whole_s}).at(0);
const double y = spline_y_.getSplineInterpolatedValues({whole_s}).at(0);

geometry_msgs::msg::Point geom_point;
geom_point.x = x;
Expand All @@ -126,8 +126,8 @@ double SplineInterpolationPoints2d::getSplineInterpolatedYaw(const size_t idx, c
whole_s = base_s_vec_.back();
}

const double diff_x = slerp_x_.getSplineInterpolatedDiffValues({whole_s}).at(0);
const double diff_y = slerp_y_.getSplineInterpolatedDiffValues({whole_s}).at(0);
const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0);
const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0);

return std::atan2(diff_y, diff_x);
}
Expand All @@ -150,6 +150,6 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner(
const auto & base_y_vec = base.at(2);

// calculate spline coefficients
slerp_x_.calcSplineCoefficients(base_s_vec_, base_x_vec);
slerp_y_.calcSplineCoefficients(base_s_vec_, base_y_vec);
spline_x_.calcSplineCoefficients(base_s_vec_, base_x_vec);
spline_y_.calcSplineCoefficients(base_s_vec_, base_y_vec);
}
16 changes: 8 additions & 8 deletions common/interpolation/test/src/test_spherical_linear_interpolation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,9 +32,9 @@ inline geometry_msgs::msg::Quaternion createQuaternionFromRPY(
}
} // namespace

TEST(spherical_linear_interpolation, slerp_scalar)
TEST(slerp, spline_scalar)
{
using interpolation::spherical_linear_interpolation;
using interpolation::slerp;

// Same value
{
Expand All @@ -46,7 +46,7 @@ TEST(spherical_linear_interpolation, slerp_scalar)
const auto ans_quat = createQuaternionFromRPY(0.0, 0.0, 0.0);

for (double ratio = -2.0; ratio < 2.0 + epsilon; ratio += 0.1) {
const auto interpolated_quat = spherical_linear_interpolation(src_quat, dst_quat, ratio);
const auto interpolated_quat = slerp(src_quat, dst_quat, ratio);
EXPECT_NEAR(ans_quat.x, interpolated_quat.x, epsilon);
EXPECT_NEAR(ans_quat.y, interpolated_quat.y, epsilon);
EXPECT_NEAR(ans_quat.z, interpolated_quat.z, epsilon);
Expand All @@ -62,7 +62,7 @@ TEST(spherical_linear_interpolation, slerp_scalar)
const auto dst_quat = createQuaternionFromRPY(0.0, 0.0, dst_yaw);

for (double ratio = -2.0; ratio < 2.0 + epsilon; ratio += 0.1) {
const auto interpolated_quat = spherical_linear_interpolation(src_quat, dst_quat, ratio);
const auto interpolated_quat = slerp(src_quat, dst_quat, ratio);

const double ans_yaw = M_PI * ratio;
tf2::Quaternion ans;
Expand All @@ -77,9 +77,9 @@ TEST(spherical_linear_interpolation, slerp_scalar)
}
}

TEST(spherical_linear_interpolation, slerp_vector)
TEST(slerp, spline_vector)
{
using interpolation::spherical_linear_interpolation;
using interpolation::slerp;

// query keys are same as base keys
{
Expand All @@ -92,7 +92,7 @@ TEST(spherical_linear_interpolation, slerp_vector)
const std::vector<double> query_keys = base_keys;
const auto ans = base_values;

const auto results = spherical_linear_interpolation(base_keys, base_values, query_keys);
const auto results = slerp(base_keys, base_values, query_keys);

for (size_t i = 0; i < results.size(); ++i) {
const auto interpolated_quat = results.at(i);
Expand Down Expand Up @@ -122,7 +122,7 @@ TEST(spherical_linear_interpolation, slerp_vector)
ans.at(4) = createQuaternionFromRPY(0.0, 0.0, 0.1 * M_PI / 5.0 + 3.0 * M_PI / 5.0);
ans.at(5) = createQuaternionFromRPY(0.0, 0.0, 0.8 * M_PI / 5.0 + 3.0 * M_PI / 5.0);

const auto results = spherical_linear_interpolation(base_keys, base_values, query_keys);
const auto results = slerp(base_keys, base_values, query_keys);

for (size_t i = 0; i < results.size(); ++i) {
const auto interpolated_quat = results.at(i);
Expand Down
Loading