点群登録 5 - フィッティング曲線を実現するための補助知識最小二乗法コード (C++)

詳細については、コンピューター ビジョン (学部) 北京郵電大学 Lu Peng Clear Complete Collection_哔哩哔哩_bilibili をご覧ください。

約45分

1. 原理説明

実験を通じて一連の観測値を取得します (3 つであると仮定します)。

W_3_n=\begin{bmatrix} w_1_1 &... &w_1_n\\ w_2_1 &... &w_2_n \\ w_3_1 & ... & w_3_m \end{bmatrix}

各サンプル観測値に対応する正確な値は次のとおりです。

y_T =\begin{bmatrix} y_1_1\\ y_1_2 \\ y_1_3 \end{bmatrix}

ここでは、観測値に対応する正確な値は次のように仮定されます。

y_p=\begin{bmatrix} y_1_1\\ y_1_2 \\y_1_3 \end{bmatrix} = \begin{bmatrix} w_1_1*x_1+w_1_2 * x_2+....+.w_1_n*x_3+b\\ w_2_1*x_1+w_2_2 * x_2+....+.w_2_n*x_3+b\\w_3_1*x_1+w_3_2 * x_2+....+.w_3_n*x_3+b \end{bmatrix}=w*X+b

上記の行列計算式は次と同等になります。

y_p=w*X+b=W*X = \begin{bmatrix} w_1_1 & ...& w_1_n&1 \\ w_1_2 & ...& w_2_n &1 \\ w_1_3 & ...& w_3_n & 1 \end{bmatrix} * \begin{bmatrix} x_1_1\\ ... \\x_1_n \\ b \end{bmatrix}

その誤差計算式は次のとおりです。

err = y_p- y_T

二乗誤差の計算式は次のとおりです。

        err^2 = err^T*err=(WX-y_T)^T*(WX-y_T)

これはx_1_1,x_1_2,...,x_1_n,b誤差公式の 2 乗公式であるため、最小誤差によれば、これは極であり、その導関数は 0 に等しくなります。

\frac{\partial }{x}err^2 = W^T( W*X-y_T)=0

次のことがわかります。

X = (W^TW)^-^1*W^You_T

この時点で係数が見つかり、それを取り込むことができます。

y_p = w_1x_1+ w_2x_2+ w_3x_3+...+w_n+x_n+b

最小二乗ジレンマ:

 

1. 近似された曲線が x 軸に垂直な直線の場合、y は存在せず、x と y の間に対応関係はなく (つまり、傾きが存在せず)、最小二乗法を求めることはできません。曲線にフィットさせるために使用されます。

2. これは回転には機能しません

重み付き最小二乗法 (x、y を一緒に考慮): 以前は、考慮された 2 つの点の間の同じ点 x に対応する y 距離の差でした。たとえば、下図の d2 でしたが、現在は、からの距離を考慮しています。点から直線までの距離。下図の d1 距離に対応します。

 

 詳細な導出については、上記のリンクを参照してください。

 

2. コードの実装

2.1 最小二乗フィッティング正弦関数

コード再現の統計の最初の章 - 最小二乗フィッティング正弦関数、正則化、その公式 Python コードは次のとおりです。

#coding:utf-8
import numpy as np
import scipy as sp
from scipy.optimize import leastsq
import matplotlib.pyplot as plt
# 目标函数
def real_func(x):
    return np.sin(2*np.pi*x)
 
# 多项式
def fit_func(p, x):
    f = np.poly1d(p)
    # print('f=',f)
    return f(x)
 
# 残差
def residuals_func(p, x, y):
    ret = fit_func(p, x) - y
    return ret
 
# 十个点
x = np.linspace(0, 1, 10)
x_points = np.linspace(0, 1, 1000)
# 加上正态分布噪音的目标函数的值
y_ = real_func(x)
y = [np.random.normal(0, 0.1) + y1 for y1 in y_]
 
 
def fitting(M=0):
    """
    M    为 多项式的次数
    """
    # 随机初始化多项式参数
    p_init = np.random.rand(M + 1)
    # 最小二乘法
    p_lsq = leastsq(residuals_func, p_init, args=(x, y))
    print('Fitting Parameters:', p_lsq[0])
    #
    # 可视化
    plt.plot(x_points, real_func(x_points), label='real')
    plt.plot(x_points, fit_func(p_lsq[0], x_points), label='fitted curve')
    plt.plot(x, y, 'bo', label='noise')
    plt.legend()
    plt.show()
    return p_lsq
# M=0
p_lsq_0 = fitting(M=0)
# M=1
p_lsq_1 = fitting(M=1)
# M=3
p_lsq_3 = fitting(M=3)
# M=9
p_lsq_9 = fitting(M=9)

その動作結果グラフ:

 2.2 C++で正弦曲線のフィッティングを実現(アピールコードM=10に対応)

ヘッダー関数:

# include<iostream>
# include<Eigen/Dense>
# include<math.h>
# include<Eigen/Eigenvalues>
using namespace std;
using namespace Eigen;

生成原理で述べた行列 w, x, yt

//生成观测值
MatrixXd x(20, 10);
//目标值
MatrixXd y(20, 1);
//权重值
MatrixXd w(10, 1) ;

誤差関数 ( err^2 = err^T*err=(WX-y_T)^T*(WX-y_T)) を計算します。

MatrixXd errCount(MatrixXd y_p, MatrixXd y_t)
{
	MatrixXd err = (y_p - y_t);
	MatrixXd myerr = err.transpose() * err;
	return myerr;

}

重要な部分 (原理) が実現されますX = (W^TW)^-^1*W^You_T:

MatrixXd leastsq(MatrixXd y_p, MatrixXd y_t, MatrixXd x, MatrixXd w)
{
	MatrixXd err = errCount(y_p, y_t);
	if (err.sum() > 0.1)
	{
		w = (x.transpose() * x).inverse() * x.transpose() * y;
		y_p = x * w;
		err = errCount(y_p, y_t);
		cout << err.sum() << endl;
	}
	return w;
}

公式関数により、再帰コンストラクター (観測値 x が 1 つだけ) であるにもかかわらず、M=10 (多項式の係数) の場合に効果が最高になることが示されています。

y = w_1x^1^0+w_2x^9+w_3x^8+w_4x^7+w_5x^6+w_6x^5+w_7x^4+w_8x^3+w_9x^2+w10x^1+b

	for (int i = 0; i < 20; i++)
	{	
		x(i, 9) = 1;
		x(i, 8)=0.25 + sum;
		x(i, 7) = x(i, 8) * x(i, 8);
		x(i, 6) = x(i, 8) * x(i, 8) * x(i, 8);
		x(i, 5) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);
		x(i, 4) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);
		x(i, 3) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);
		x(i, 2) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);
		x(i, 1) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8)*x(i, 8);
		x(i, 0) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8)*x(i, 8)*x(i, 8);
		y(i, 0) = sin(x(i,8)*3.14);
		sum = sum + 0.25;
	}
w:
          0           0           0           0           0           0           0           0           0           1
 3.8147e-06 1.52588e-05 6.10352e-05 0.000244141 0.000976562  0.00390625    0.015625      0.0625        0.25           1
 0.00195312  0.00390625   0.0078125    0.015625     0.03125      0.0625       0.125        0.25         0.5           1
  0.0750847    0.100113    0.133484    0.177979    0.237305    0.316406    0.421875      0.5625        0.75           1
          1           1           1           1           1           1           1           1           1           1
    7.45058     5.96046     4.76837      3.8147     3.05176     2.44141     1.95312      1.5625        1.25           1
    38.4434     25.6289     17.0859     11.3906     7.59375      5.0625       3.375        2.25         1.5           1
    153.937     87.9639     50.2651     28.7229     16.4131     9.37891     5.35938      3.0625        1.75           1
        512         256         128          64          32          16           8           4           2           1
    1477.89     656.841     291.929     129.746      57.665     25.6289     11.3906      5.0625        2.25           1
     3814.7     1525.88     610.352     244.141     97.6562     39.0625      15.625        6.25         2.5           1
    8994.86     3270.86      1189.4      432.51     157.276     57.1914     20.7969      7.5625        2.75           1
      19683        6561        2187         729         243          81          27           9           3           1
      40453     12447.1     3829.87     1178.42     362.591     111.566     34.3281     10.5625        3.25           1
    78815.6     22518.8     6433.93     1838.27     525.219     150.062      42.875       12.25         3.5           1
     146650     39106.6     10428.4     2780.91     741.577     197.754     52.7344     14.0625        3.75           1
     262144       65536       16384        4096        1024         256          64          16           4           1
     452377      106442     25045.1     5892.96     1386.58     326.254     76.7656     18.0625        4.25           1
     756681      168151     37366.9     8303.77     1845.28     410.062      91.125       20.25         4.5           1
1.23096e+06      259149     54557.6     11485.8     2418.07     509.066     107.172     22.5625        4.75           1
yt:
          0    0.706825           1    0.707951  0.00159265   -0.705698   -0.999997   -0.709075  -0.0031853    0.704568    0.999992    0.710197  0.00477794   -0.703437   -0.999984   -0.711317 -0.00637057    0.702304    0.999974    0.712436
原始y_p:
          1     1.33333     1.99805     3.77475          10     33.2529      113.33     357.853        1023     2659.41     6357.16     14134.2       29524     58431.6      110341      199977      349525      591569      972875 1.55921e+06

変換後:

精确值
          0    0.706825           1    0.707951  0.00159265   -0.705698   -0.999997   -0.709075  -0.0031853    0.704568    0.999992    0.710197  0.00477794   -0.703437   -0.999984   -0.711317 -0.00637057    0.702304    0.999974    0.712436
变换后的y_p:
   0.0103961     0.658876      1.06156     0.724083   -0.0521667    -0.743825    -0.969841    -0.649122    0.0135521     0.657116     0.942181     0.709952    0.0648353    -0.655788     -1.02993    -0.781023 -0.000910797     0.785843      0.94112     0.724696

2.3 完全なコード

# include<iostream>
# include<Eigen/Dense>
# include<math.h>
# include<Eigen/Eigenvalues>
using namespace std;
using namespace Eigen;
//生成观测值
MatrixXd x(20, 10);
//目标值
MatrixXd y(20, 1);
//权重值
MatrixXd w(10, 1) ;

MatrixXd errCount(MatrixXd y_p, MatrixXd y_t)
{
	MatrixXd err = (y_p - y_t);
	MatrixXd myerr = err.transpose() * err;
	return myerr;

}
MatrixXd leastsq(MatrixXd y_p, MatrixXd y_t, MatrixXd x, MatrixXd w)
{
	MatrixXd err = errCount(y_p, y_t);
	if (err.sum() > 0.1)
	{
		w = (x.transpose() * x).inverse() * x.transpose() * y;
		y_p = x * w;
		err = errCount(y_p, y_t);
		cout << err.sum() << endl;
	}
	return w;
}

int main()
{
	float sum=-0.25;
	//给观测值和准确值赋值
	for (int i = 0; i < 20; i++)
	{	
		x(i, 9) = 1;
		x(i, 8)=0.25 + sum;
		x(i, 7) = x(i, 8) * x(i, 8);
		x(i, 6) = x(i, 8) * x(i, 8) * x(i, 8);
		x(i, 5) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);
		x(i, 4) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);
		x(i, 3) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);
		x(i, 2) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8);
		x(i, 1) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8)*x(i, 8);
		x(i, 0) = x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8) * x(i, 8)*x(i, 8)*x(i, 8);
		y(i, 0) = sin(x(i,8)*3.14);
		sum = sum + 0.25;
	}
	cout << "w:" << endl;
	cout << x << endl;
	cout << "yt:" << endl;
	cout << y.transpose() << endl;
	w << 1, 1, 1, 1, 1, 1, 1, 1, 1, 1;
	cout << "原始y_p:" << endl;
	cout << (x * w).transpose() << endl;
	MatrixXd y_p = x * w;
	w = leastsq(y_p, y, x, w);
	cout << "精确值" << endl;
	cout << y.transpose() << endl;
	cout << "变换后的y_p:" << endl;
	cout << (x * w).transpose() << endl;

}

3. まとめ

アピール公式によると変身は1回だけらしいのですが、ちょっと不思議です。実験により、たった 1 回変更しただけでは誤差が非常に小さいことが証明されました。

おすすめ

転載: blog.csdn.net/qq_40214464/article/details/121447609