基于最小二乘法的多項(xiàng)式曲線擬合:從原理到c++實(shí)現(xiàn)
在自動(dòng)駕駛系統(tǒng)中,通常會(huì)用起點(diǎn)、終點(diǎn)和一個(gè)三階多項(xiàng)式來表示一條車道線,多項(xiàng)式系數(shù)的求解一般用最小二乘法來實(shí)現(xiàn)。
本文首先介紹兩種基于最小二乘法的多項(xiàng)式擬合方法的原理,然后基于OpenCV用c++編寫了這兩種擬合方法的代碼,最后通過一個(gè)完整的示例來展示如何通過一個(gè)離散點(diǎn)集擬合出一條多項(xiàng)式曲線。
基于最小二乘法的多項(xiàng)式擬合原理推導(dǎo)代數(shù)方式求解多項(xiàng)式曲線擬合是指基于一系列的觀測(cè)點(diǎn)去尋找一個(gè)多項(xiàng)式來表示這些點(diǎn)的關(guān)系,最小二乘法通過最小化誤差的平方和去尋找數(shù)據(jù)的最佳匹配函數(shù)。假設(shè)有點(diǎn)集
其中,和的關(guān)系滿足函數(shù)
假設(shè)次多項(xiàng)式函數(shù)為
其中為多項(xiàng)式系數(shù)。如果要用這個(gè)次多項(xiàng)式來表示與的關(guān)系,那么多項(xiàng)式值與真實(shí)值之間的誤差為
采用最小二乘法進(jìn)行多項(xiàng)式擬合的目的就是尋找一組最佳的多項(xiàng)式系數(shù)使得擬合后整個(gè)點(diǎn)集的總誤差最小,而求總誤差最小的問題可以轉(zhuǎn)化為求誤差平方和最小。整個(gè)點(diǎn)集的誤差平方和為
要使最小,可以對(duì)()求偏導(dǎo)并令其為零:
可得
寫成矩陣形式可得
把上式寫為,那么只要計(jì)算出
和
,再代入上面的式子中構(gòu)建出矩陣和,那么多項(xiàng)式系數(shù)就可以通過求解線性方程組得到。
矩陣方式求解從上一節(jié)的推導(dǎo)過程可以看到,用代數(shù)法求解多項(xiàng)式系數(shù)的方法非常繁瑣而且計(jì)算量比較大,我們其實(shí)還可以用矩陣求解的方法來簡(jiǎn)化計(jì)算過程。
令
那么整個(gè)點(diǎn)集的誤差平方和可以表示為
對(duì)求導(dǎo)可得
令導(dǎo)數(shù)為零,那么可以得到
解得
多項(xiàng)式曲線擬合的C++代碼實(shí)現(xiàn)
學(xué)習(xí)了前面的理論知識(shí),我們用c++來編碼實(shí)現(xiàn)一下,畢竟光看一堆數(shù)學(xué)公式還是不夠直觀。從前面的理論知識(shí)可以知道,用最小二乘法做多項(xiàng)式曲線擬合其實(shí)質(zhì)就是一個(gè)矩陣求解的問題,我們可以借助一些常用的數(shù)學(xué)庫(kù)比如OpenCV或Eigen來求解。本文將采用OpenCV來實(shí)現(xiàn)。
1. 代數(shù)方式求解的C++代碼實(shí)現(xiàn)如果理解了前面的理論,寫代碼其實(shí)就比較簡(jiǎn)單了。對(duì)照前面理論部分的公式,構(gòu)建好矩陣A和B,然后調(diào)用OpenCV的solve()函數(shù)來求解即可:
// 代數(shù)方式求解2. 矩陣方式求解的C++代碼實(shí)現(xiàn)
void PolyFit(const std::vector<cv::Point2f> &points, const int order,
cv::Mat *coeff) {
const int n = points.size();
cv::Mat A = cv::Mat::zeros(order + 1, order + 1, CV_64FC1);
cv::Mat B = cv::Mat::zeros(order + 1, 1, CV_64FC1);
// 構(gòu)建A矩陣
for (int i = 0; i < order + 1; ++i) {
for (int j = 0; j < order + 1; ++j) {
for (int k = 0; k < n; ++k) {
A.at<double>(i, j) += std::pow(points.at(k).x, i + j);
}
}
}
// 構(gòu)建B矩陣
for (int i = 0; i < order + 1; ++i) {
for (int k = 0; k < n; ++k) {
B.at<double>(i, 0) += std::pow(points.at(k).x, i) * points.at(k).y;
}
}
(*coeff) = cv::Mat::zeros(order + 1, 1, CV_64FC1);
// 求解
if (!cv::solve(A, B, *coeff, cv::DECOMP_LU)) {
std::cout << "Failed to solve !" << std::endl;
}
}
矩陣方式求解的代碼更加簡(jiǎn)單:
// 矩陣方式求解3. 一個(gè)完整的多項(xiàng)式曲線擬合示例
void PolyFit(const std::vector<cv::Point2f> &points, const int order,
cv::Mat *coeff) {
const int n = points.size();
cv::Mat A = cv::Mat::ones(n, order + 1, CV_64FC1);
cv::Mat B = cv::Mat::zeros(n, 1, CV_64FC1);
for (int i = 0; i < n; ++i) {
const double a = points.at(i).x;
const double b = points.at(i).y;
B.at<double>(i, 0) = b;
if (i > 0) {
for (int j = 1, v = a; j < order + 1; ++j, v *= a) {
A.at<double>(i, j) = v;
}
}
}
(*coeff) = (A.t() * A).inv() * A.t() * B;
}
接下來,我們來看一個(gè)簡(jiǎn)單的曲線擬合示例:
int main(int argc, char **argv) {
constexpr int kOrder = 3; // 多項(xiàng)式階數(shù)
constexpr int kWidth = 1000;
constexpr int kHeight = 500;
cv::Mat canvas =
cv::Mat(cv::Size(kWidth, kHeight), CV_8UC3, cv::Scalar(255, 255, 255));
cv::RNG rng(0xFFFFFFFF); // 隨機(jī)數(shù)
// 生成點(diǎn)集,y坐標(biāo)添加一些隨機(jī)噪聲
std::vector<cv::Point2f> raw_points;
for (int i = 10; i < kWidth; i += 10) {
cv::Point2f p;
p.x = i;
const auto noise = rng.uniform(-kHeight / 10, kHeight / 10);
p.y = kHeight - p.x / kWidth * kHeight + noise;
cv::circle(canvas, p, 5, cv::Scalar(0, 0, 255), -1);
raw_points.emplace_back(p);
}
// 多項(xiàng)式擬合
cv::Mat coeff;
PolyFit(raw_points, kOrder, &coeff);
// 用擬合后的系數(shù)重新生成點(diǎn)集
std::vector<cv::Point> poly_points;
for (const auto &rp : raw_points) {
cv::Point p;
p.x = rp.x;
p.y = PolyValue(coeff, kOrder, rp.x);
cv::circle(canvas, p, 5, cv::Scalar(0, 255, 0), -1);
poly_points.emplace_back(p);
}
cv::polylines(canvas, poly_points, false, cv::Scalar(0, 255, 0), 3,
cv::LINE_4);
cv::imshow("PolyFit", canvas);
cv::waitKey(0);
cv::destroyAllWindows();
return 0;
}
代碼的流程如下:
- 生成一個(gè)離散點(diǎn)集,其中每個(gè)點(diǎn)的y坐標(biāo)被添加一定的隨機(jī)噪聲;
- 調(diào)用前面實(shí)現(xiàn)的多項(xiàng)式擬合函數(shù)求解多項(xiàng)式系數(shù);
- 用求解出的多項(xiàng)式系數(shù)重新計(jì)算每個(gè)點(diǎn)的y坐標(biāo)值;
- 可視化原始點(diǎn)集和擬合后的點(diǎn)集。
其中用于計(jì)算多項(xiàng)式值的函數(shù)PolyValue()實(shí)現(xiàn)如下:
float PolyValue(const cv::Mat &coeff, const int order, const float x) {
float v = 0;
for (int i = 0; i < order; ++i) {
v += coeff.at<double>(i, 0) * std::pow(x, i);
}
return v;
}
可視化的結(jié)果如下圖所示,其中紅色點(diǎn)為原始點(diǎn),綠色點(diǎn)為擬合后的點(diǎn)。
總結(jié)本文詳細(xì)介紹了基于最小二乘法的多項(xiàng)式擬合的原理和代碼實(shí)現(xiàn),希望對(duì)各位讀者有所幫助。如果對(duì)本文內(nèi)容有疑問,歡迎評(píng)論區(qū)留言與我交流。
*博客內(nèi)容為網(wǎng)友個(gè)人發(fā)布,僅代表博主個(gè)人觀點(diǎn),如有侵權(quán)請(qǐng)聯(lián)系工作人員刪除。