最小二乘法进行最高3次曲线拟合
最近在做跟踪时，需要预测被跟踪物体的运动轨迹，由于被跟踪物体为车辆，轨迹使用二次曲线基本可以较好的拟合，因此做一下实验。
下面为最小二乘法的核心代码，有需要可以参考：
bool CNXMinSquare::Calc(std::vector
{
vtCoef.clear();
// 1. 根据函数值对 1 x x^2 x^3 施密特正交化
// 多项式使用Mat表示，行向量，底下标表示低指数的系数
const int icRow = 1;
const int icCol = 4;
std::vector
vtBase.reserve(icCol);
for(int i = 0; i < icCol; ++i)
{
cv::Mat matBase(icRow, icCol, CV_64F, cv::Scalar(0.0));
matBase.at
vtBase.push_back(matBase);
}
std::vector
vtSchmidt.reserve(icCol);
cv::Mat matOne(icRow, icCol, CV_64F, cv::Scalar(0.0));
matOne.at
vtSchmidt.push_back(matOne);
unsigned char *pucData = NULL;
std::vector
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 = vtSchmidtDotSelf[k];
}
else
{
dbDenominator = _FunDot(vtSchmidt[k], vtSchmidt[k], vtPoint, true); // 分母
vtSchmidtDotSelf.push_back(dbDenominator);
}
double dbNumerator = _FunDot(vtSchmidt[k], vtBase[i], vtPoint, true);// 分子
if(fabs(dbNumerator) <= 0.0000001)
{
continue;
}
matSum = matSum + vtSchmidt[k] * (dbNumerator / dbDenominator);
}
vtSchmidt.push_back(vtBase[i] - matSum);
}
vtSchmidtDotSelf.push_back(_FunDot(vtSchmidt[icCol - 1], vtSchmidt[icCol - 1], vtPoint, true));
// 2. 计算系数
cv::Mat matFun(icRow, vtPoint.size(), CV_64F);
for(int i = 0; i < matFun.cols; ++i)
{
matFun.at
}
vtCoef.reserve(icCol);
for(int i = 0; i < icCol; ++i)
{
double dbUp = _FunDot(matFun, vtSchmidt[i], vtPoint, false);
if(fabs(dbUp) <= 0.0000001)
{
vtCoef.push_back(0.0);
}
else
{
double dbCoef = dbUp / vtSchmidtDotSelf[i];
vtCoef.push_back(dbCoef);
}
}
cv::Mat matPoly(icRow, icCol, CV_64F, cv::Scalar(0.0));
for(int i = 0; i < icCol; ++i)
{
matPoly = matPoly + vtSchmidt[i] * vtCoef[i];
}
for(int i = 0; i < icCol; ++i)
{
vtCoef[i] = matPoly.at
}
return true;
}
通过实验对比，使用19个点做拟合，预测下一个点的位置，部分实验数据如下：
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 19 : 770] RealPoint: [707, 148] PredictPoint: [707, 148]
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 20 : 235] Coef[0]: -28397.224449
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 20 : 235] Coef[1]: 115.290783
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 20 : 235] Coef[2]: -0.157550
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 20 : 235] Coef[3]: 0.000072
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 20 : 235] RealPoint: [705, 149] PredictPoint: [705, 149]
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 20 : 701] Coef[0]: -29718.170621
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 20 : 701] Coef[1]: 121.194305
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 20 : 701] Coef[2]: -0.166293
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 20 : 701] Coef[3]: 0.000076
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 20 : 701] RealPoint: [702, 151] PredictPoint: [702, 151]
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 21 : 169] Coef[0]: -27966.682421
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 21 : 169] Coef[1]: 114.179202
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 21 : 169] Coef[2]: -0.156940
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 21 : 169] Coef[3]: 0.000072
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 21 : 169] RealPoint: [700, 152] PredictPoint: [700, 152]
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 21 : 731] Coef[0]: -10295.078061
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 21 : 731] Coef[1]: 40.777546
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 21 : 731] Coef[2]: -0.055324
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 21 : 731] Coef[3]: 0.000025
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 21 : 731] RealPoint: [697, 154] PredictPoint: [697, 154]
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 22 : 236] Coef[0]: 14219.857894
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 22 : 236] Coef[1]: -61.894931
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 22 : 236] Coef[2]: 0.087990
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 22 : 236] Coef[3]: -0.000041
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 22 : 236] RealPoint: [695, 156] PredictPoint: [695, 155]
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 22 : 707] Coef[0]: -1227.219474
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 22 : 707] Coef[1]: 2.342881
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 22 : 707] Coef[2]: -0.001041
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 22 : 707] Coef[3]: -0.000000
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 22 : 707] RealPoint: [692, 159] PredictPoint: [692, 157]
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 23 : 134] Coef[0]: -26314.113271
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 23 : 134] Coef[1]: 107.016657
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 23 : 134] Coef[2]: -0.146597
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 23 : 134] Coef[3]: 0.000067
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 23 : 134] RealPoint: [690, 160] PredictPoint: [690, 160]
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 23 : 610] Coef[0]: -34341.212309
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 23 : 610] Coef[1]: 141.058173
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 23 : 610] Coef[2]: -0.194708
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 23 : 610] Coef[3]: 0.000090
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 23 : 610] RealPoint: [687, 162] PredictPoint: [687, 162]
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 24 : 44] Coef[0]: -33288.851783
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 24 : 44] Coef[1]: 137.156023
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 24 : 44] Coef[2]: -0.189979
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 24 : 44] Coef[3]: 0.000088
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 24 : 44] RealPoint: [685, 164] PredictPoint: [685, 164]
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 24 : 475] Coef[0]: -22741.362937
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 24 : 475] Coef[1]: 92.270965
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 24 : 475] Coef[2]: -0.126320
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 24 : 475] Coef[3]: 0.000058
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 24 : 475] RealPoint: [682, 167] PredictPoint: [682, 166]
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 24 : 949] Coef[0]: -39783.406747
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 24 : 949] Coef[1]: 164.918557
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 24 : 949] Coef[2]: -0.229532
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 24 : 949] Coef[3]: 0.000107
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 24 : 949] RealPoint: [680, 168] PredictPoint: [680, 168]
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 25 : 425] Coef[0]: -16196.141728
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 25 : 425] Coef[1]: 64.350356
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 25 : 425] Coef[2]: -0.086624
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 25 : 425] Coef[3]: 0.000039
[PID: 32412 TID: 32068] [2016 : 12 : 20 # 13 : 25 : 25 : 425] RealPoint: [676, 170] PredictPoint: [676, 172]
可以看出真实值和预测值基本一致，更多的数据可以看出Y方向的像素值偏差的绝对值一般在3以内，可以达到要求的精度。
从数据中可以看出拟合的曲线为： y = a + bx + cx^2，即一个二次曲线。
由于是一个局部拟合，因此，得到的曲线只能预测已知点附近的值，不能做大范围远距离的预测。
如果要做持续跟踪预测，则需要不断更新用于拟合的19个点即可。