update abstract_interpolation
This commit is contained in:
parent
de82c76f94
commit
8c40281ef9
@ -1,31 +1,72 @@
|
|||||||
#ifndef INTERPOLATION_BASE_H
|
#ifndef INTERPOLATION_BASE_H
|
||||||
#define INTERPOLATION_BASE_H
|
#define INTERPOLATION_BASE_H
|
||||||
|
|
||||||
|
#include "utils/math/geometry.h"
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include <stdexcept>
|
#include <stdexcept>
|
||||||
|
|
||||||
// 插值基类
|
namespace cmvr::math {
|
||||||
class AbstractInterpolation {
|
|
||||||
protected:
|
|
||||||
std::vector<double> x_points; // 已知点的x坐标
|
|
||||||
std::vector<double> y_points; // 已知点的y坐标
|
|
||||||
|
|
||||||
// 查找x所在的区间索引（二分查找）
|
/**
|
||||||
int findInterval(double x) const;
|
* 插值基类，用于机械臂位姿插值
|
||||||
|
* 支持三维位姿(Pose3d)和二维位姿(Pose2d)的插值
|
||||||
public:
|
*/
|
||||||
|
class AbstractInterpolation {
|
||||||
|
public:
|
||||||
// 构造函数
|
// 构造函数
|
||||||
AbstractInterpolation(const std::vector<double>& x, const std::vector<double>& y);
|
AbstractInterpolation(const std::vector<Pose3d>& waypoints);
|
||||||
|
AbstractInterpolation(const std::vector<Pose2d>& waypoints);
|
||||||
|
|
||||||
// 析构函数
|
// 析构函数
|
||||||
virtual ~AbstractInterpolation() = default;
|
virtual ~AbstractInterpolation() = default;
|
||||||
|
|
||||||
// 纯虚函数：单个点插值
|
/**
|
||||||
virtual double interpolate(double x) const = 0;
|
* 插值计算接口
|
||||||
|
* @param t 插值参数，范围[0, 1]，0表示起点，1表示终点
|
||||||
|
* @return 插值得到的位姿
|
||||||
|
*/
|
||||||
|
virtual Pose3d interpolate_3d(double t) const = 0;
|
||||||
|
virtual Pose2d interpolate_2d(double t) const = 0;
|
||||||
|
|
||||||
// 批量插值（默认实现）
|
/**
|
||||||
virtual std::vector<double> interpolate(const std::vector<double>& x_values) const;
|
* 生成轨迹点序列
|
||||||
};
|
* @param num_points 生成的轨迹点数量，包括起点和终点
|
||||||
|
* @return 轨迹点序列
|
||||||
|
*/
|
||||||
|
std::vector<Pose3d> generate_trajectory_3d(size_t num_points) const;
|
||||||
|
std::vector<Pose2d> generate_trajectory_2d(size_t num_points) const;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
// 路径点存储
|
||||||
|
std::vector<Pose3d> waypoints_3d_;
|
||||||
|
std::vector<Pose2d> waypoints_2d_;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* 辅助函数：线性插值单个数值
|
||||||
|
* @param a 起始值
|
||||||
|
* @param b 目标值
|
||||||
|
* @param t 插值参数[0,1]
|
||||||
|
* @return 插值结果
|
||||||
|
*/
|
||||||
|
static double linear_interpolate(double a, double b, double t);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* 辅助函数：四元数球面线性插值(slerp)
|
||||||
|
* @param q1 起始四元数
|
||||||
|
* @param q2 目标四元数
|
||||||
|
* @param t 插值参数[0,1]
|
||||||
|
* @return 插值结果
|
||||||
|
*/
|
||||||
|
static Quat slerp(const Quat& q1, const Quat& q2, double t);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* 检查t的范围并进行约束
|
||||||
|
* @param t 原始参数
|
||||||
|
* @return 约束在[0,1]范围内的参数
|
||||||
|
*/
|
||||||
|
static double clamp_t(double t);
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace cmvr::math
|
||||||
|
|
||||||
#endif // INTERPOLATION_BASE_H
|
#endif // INTERPOLATION_BASE_H
|
||||||
|
|
||||||
@ -1,73 +1,107 @@
|
|||||||
#include "utils/base/abstract_interpolation.h"
|
#include "utils/base/abstract_interpolation.h"
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <stdexcept>
|
#include <cmath>
|
||||||
|
|
||||||
|
namespace cmvr::math {
|
||||||
|
|
||||||
// 构造函数实现
|
// 构造函数实现
|
||||||
AbstractInterpolation::AbstractInterpolation(const std::vector<double>& x, const std::vector<double>& y) {
|
AbstractInterpolation::AbstractInterpolation(const std::vector<Pose3d>& waypoints)
|
||||||
// 验证输入
|
: waypoints_3d_(waypoints) {
|
||||||
if (x.size() != y.size()) {
|
if (waypoints.size() < 2) {
|
||||||
throw std::invalid_argument("x和y的长度必须相同");
|
throw std::invalid_argument("至少需要2个路径点才能进行插值");
|
||||||
}
|
|
||||||
if (x.size() < 2) {
|
|
||||||
throw std::invalid_argument("至少需要2个已知点才能进行插值");
|
|
||||||
}
|
|
||||||
|
|
||||||
// 复制并排序数据点
|
|
||||||
std::vector<std::pair<double, double>> points;
|
|
||||||
points.reserve(x.size());
|
|
||||||
|
|
||||||
for (size_t i = 0; i < x.size(); ++i) {
|
|
||||||
points.emplace_back(x[i], y[i]);
|
|
||||||
}
|
|
||||||
|
|
||||||
// 按x坐标排序
|
|
||||||
std::sort(points.begin(), points.end());
|
|
||||||
|
|
||||||
// 分离排序后的x和y
|
|
||||||
x_points.reserve(points.size());
|
|
||||||
y_points.reserve(points.size());
|
|
||||||
|
|
||||||
for (const auto& p : points) {
|
|
||||||
x_points.push_back(p.first);
|
|
||||||
y_points.push_back(p.second);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// 查找区间索引实现
|
AbstractInterpolation::AbstractInterpolation(const std::vector<Pose2d>& waypoints)
|
||||||
int AbstractInterpolation::findInterval(double x) const {
|
: waypoints_2d_(waypoints) {
|
||||||
// 处理边界情况
|
if (waypoints.size() < 2) {
|
||||||
if (x <= x_points[0]) {
|
throw std::invalid_argument("至少需要2个路径点才能进行插值");
|
||||||
return 0;
|
|
||||||
}
|
}
|
||||||
if (x >= x_points.back()) {
|
|
||||||
return static_cast<int>(x_points.size()) - 2;
|
|
||||||
}
|
|
||||||
|
|
||||||
// 二分查找
|
|
||||||
int left = 0;
|
|
||||||
int right = static_cast<int>(x_points.size()) - 1;
|
|
||||||
|
|
||||||
while (right - left > 1) {
|
|
||||||
int mid = (left + right) / 2;
|
|
||||||
if (x < x_points[mid]) {
|
|
||||||
right = mid;
|
|
||||||
} else {
|
|
||||||
left = mid;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
return left;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// 批量插值实现
|
// 生成3D轨迹
|
||||||
std::vector<double> AbstractInterpolation::interpolate(const std::vector<double>& x_values) const {
|
std::vector<Pose3d> AbstractInterpolation::generate_trajectory_3d(size_t num_points) const {
|
||||||
std::vector<double> results;
|
if (num_points < 2) {
|
||||||
results.reserve(x_values.size());
|
throw std::invalid_argument("轨迹点数量至少为2");
|
||||||
|
|
||||||
for (double x : x_values) {
|
|
||||||
results.push_back(interpolate(x));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return results;
|
std::vector<Pose3d> trajectory;
|
||||||
|
trajectory.reserve(num_points);
|
||||||
|
|
||||||
|
for (size_t i = 0; i < num_points; ++i) {
|
||||||
|
double t = static_cast<double>(i) / (num_points - 1);
|
||||||
|
trajectory.push_back(interpolate_3d(t));
|
||||||
|
}
|
||||||
|
|
||||||
|
return trajectory;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 生成2D轨迹
|
||||||
|
std::vector<Pose2d> AbstractInterpolation::generate_trajectory_2d(size_t num_points) const {
|
||||||
|
if (num_points < 2) {
|
||||||
|
throw std::invalid_argument("轨迹点数量至少为2");
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<Pose2d> trajectory;
|
||||||
|
trajectory.reserve(num_points);
|
||||||
|
|
||||||
|
for (size_t i = 0; i < num_points; ++i) {
|
||||||
|
double t = static_cast<double>(i) / (num_points - 1);
|
||||||
|
trajectory.push_back(interpolate_2d(t));
|
||||||
|
}
|
||||||
|
|
||||||
|
return trajectory;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 线性插值辅助函数
|
||||||
|
double AbstractInterpolation::linear_interpolate(double a, double b, double t) {
|
||||||
|
t = clamp_t(t);
|
||||||
|
return a + t * (b - a);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 四元数球面线性插值
|
||||||
|
Quat AbstractInterpolation::slerp(const Quat& q1, const Quat& q2, double t) {
|
||||||
|
t = clamp_t(t);
|
||||||
|
|
||||||
|
Quat result;
|
||||||
|
double dot = q1.w * q2.w + q1.x * q2.x + q1.y * q2.y + q1.z * q2.z;
|
||||||
|
|
||||||
|
// 确保我们选择最短路径
|
||||||
|
if (dot < 0.0) {
|
||||||
|
Quat q2_neg = {-q2.w, -q2.x, -q2.y, -q2.z};
|
||||||
|
return slerp(q1, q2_neg, t);
|
||||||
|
}
|
||||||
|
|
||||||
|
// 防止数值问题，接近时使用线性插值
|
||||||
|
if (dot > 0.9995) {
|
||||||
|
result.w = linear_interpolate(q1.w, q2.w, t);
|
||||||
|
result.x = linear_interpolate(q1.x, q2.x, t);
|
||||||
|
result.y = linear_interpolate(q1.y, q2.y, t);
|
||||||
|
result.z = linear_interpolate(q1.z, q2.z, t);
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 计算角度和插值系数
|
||||||
|
double theta_0 = std::acos(dot);
|
||||||
|
double theta = theta_0 * t;
|
||||||
|
double sin_theta = std::sin(theta);
|
||||||
|
double sin_theta_0 = std::sin(theta_0);
|
||||||
|
|
||||||
|
double s1 = std::cos(theta) - dot * sin_theta / sin_theta_0;
|
||||||
|
double s2 = sin_theta / sin_theta_0;
|
||||||
|
|
||||||
|
result.w = s1 * q1.w + s2 * q2.w;
|
||||||
|
result.x = s1 * q1.x + s2 * q2.x;
|
||||||
|
result.y = s1 * q1.y + s2 * q2.y;
|
||||||
|
result.z = s1 * q1.z + s2 * q2.z;
|
||||||
|
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 参数范围约束
|
||||||
|
double AbstractInterpolation::clamp_t(double t) {
|
||||||
|
return std::max(0.0, std::min(1.0, t));
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace cmvr::math
|
||||||
|
|
||||||
Loading…
Reference in New Issue
Block a user