最小二乘法进行最高3次曲线拟合

最新推荐文章于 2025-10-07 23:47:17 发布

原创

最新推荐文章于 2025-10-07 23:47:17 发布 · 1.1w 阅读

13 ·

CC 4.0 BY-SA版权

文章标签：

#曲线拟合 #最小二乘法 #预测

这篇博客介绍了如何使用最小二乘法来对车辆轨迹进行最高3次的曲线拟合，以预测物体的运动轨迹。通过实验，展示了拟合过程中的系数计算，并且验证了预测点与真实点之间的误差通常在3以内，达到了预期的精度。这种方法适用于局部预测，但不适合远距离预测，持续跟踪需要不断更新拟合点。

最近在做跟踪时，需要预测被跟踪物体的运动轨迹，由于被跟踪物体为车辆，轨迹使用二次曲线基本可以较好的拟合，因此做一下实验。

下面为最小二乘法的核心代码，有需要可以参考：

bool CNXMinSquare::Calc(std::vector<double> &vtCoef, std::vector<cv::Point2d> &vtPoint)
{
vtCoef.clear();
// 1. 根据函数值对 1 x x^2 x^3 施密特正交化
// 多项式使用Mat表示，行向量，底下标表示低指数的系数
const int icRow = 1;
const int icCol = 4;
std::vector<cv::Mat> vtBase; // 原始基础基
vtBase.reserve(icCol);
for(int i = 0; i < icCol; ++i)
{
cv::Mat matBase(icRow, icCol, CV_64F, cv::Scalar(0.0));
matBase.at<double>(i) = 1.0;
vtBase.push_back(matBase);
}

std::vector<cv::Mat> vtSchmidt; // 施密特正交化基
vtSchmidt.reserve(icCol);
cv::Mat matOne(icRow, icCol, CV_64F, cv::Scalar(0.0));
matOne.at<double>(0) = 1.0;
vtSchmidt.push_back(matOne);

unsigned char *pucData = NULL;
std::vector<double> vtSchmidtDotSelf;
for(int i = 1; i < icCol; ++i)
{
cv::Mat matSum(icRow, icCol, CV_64F, cv::Scalar(0.0));
for(int k = 0; k < i; ++k)
{
double dbDenominator = 0.0;
if(vtSchmidtDotSelf.size() > k)
{
dbDenominator = vtSchmidtDotSel

最低0.47元/天解锁文章