关于原⽂下⾯评论中的问题：为什么最后⼀步明明是求解线性⽅程组AX=Y(X， Y均为向量)中的系数矩阵A， ⽽A的表达式为A = (X'*X)-1*X'*Y，这是⼀个超定⽅程组，A矩阵是奇异矩阵，所以A并不可直接求逆，只能求伪逆。
矩阵计算部分⽤了Eigen库，其中的输⼊vector中的数据类型MyPoint是我⾃⼰定义的X,Y,Z点类型，具体见github。 compute⽅法输⼊为同⼀平⾯上的点，⽤于估计系数矩阵A； 具体拟合到⼏次⽅可以设置_POWER
#include <iostream>
#include <vector>
#include <math.h>
#include <fstream>
#include <sstream>
#include "Eigen/Eigen"
#include <Eigen/SVD>
int _POWER; //参数个数， _POWER-1等于拟合的最⾼次数
class LeastSquare {
private:
Eigen::RowVectorXd coeficients;//系数矩阵
double Deviation;
double k, b;
public:
LeastSquare(const vector<MyPoint> &ptr) {//ptr是⼀条线的点云数据
coeficients.setZero();//初始化为0矩阵
compute(ptr);//计算估计参数矩阵，拟合曲线；
拟合直线
//compute1(ptr);
}
double getk() { return k; }
double getb() { return b; }
double power(const double number, int power)
{
double temp = 1;
while (power--)
{
temp *= number;
}
return temp;
}
void compute1(const vector<MyPoint> &ptr);//compute the k and b;
void compute(const vector<MyPoint> &ptr);parameter estimate---compute coeficients
void filter(const vector<MyPoint> &ptr, vector<MyPoint> &outlier);
void fucking_print()
{
//cout << "y = " << k << "x + " << b << "\t";
cout << "曲线的多项式系数（形如y = a0 + a1*x + a2*x2 +...am*xm）为:\n" << coeficients << endl;
}
};
void LeastSquare::compute1(const vector<MyPoint> &ptr) //compute k and b，这个是之前写的拟合直线的，下⾯的多项式拟合次数取2和这个⽅法结果⼀致{
double SumXi = 0.0;
double SumYi = 0.0;
double SumXi_2 = 0.0;
double SumXiYi = 0.0;
double deviation = 0.0;
int number = int(ptr.size());//size(返回的是⽆符号整型，需要强制类型转换后使⽤)
for (int i = 0; i < number;i++)
{
SumXi += ptr[i].x;
SumXi += ptr[i].x;
SumYi += ptr[i].z;
SumXi_2 += ptr[i].x * ptr[i].x;
SumXiYi += ptr[i].x * ptr[i].z;
}
double temp = (number * SumXi_2 - SumXi * SumXi);//减少计算次数
k = (number * SumXiYi - SumXi * SumYi) / temp;
b = (SumXi_2 * SumYi - SumXi * SumXiYi) / temp;
}
void LeastSquare::compute(const vector<MyPoint> &ptr) //多项式拟合的参数估计
{
if (_POWER < 2) return; //参数⾄少两个
const int PointNum = int(ptr.size());
Eigen::RowVectorXd Xi_1(PointNum);//Xi的⼀次矩阵，1⾏n列
Eigen::RowVectorXd Yi_1(PointNum);//Yi的⼀次矩阵，1⾏n列
Eigen::MatrixXd A(PointNum, _POWER);//Ax = b中的A，结构矩阵，x为系数矩阵coeficients，b为数据矩阵，即Yi向量（1 * n的矩阵）,power为次数 Xi_1.setZero();
Yi_1.setZero();
A.setOnes();
for (unsigned int i = 0; i < ptr.size(); i++)//Ps：⾏的下标对应⽂件的⾏数-2，因为读⽂件的时候第⼀⾏会被忽略，然后下标⼜是从0开始
{
Xi_1[i] = ptr[i].x;
Yi_1[i] = ptr[i].z;
for (int cols = 0; cols < _POWER; cols++)
{
A(i, cols) = power(ptr[i].x, cols);
}
//A(i, _POWER - 1) = ptr[i].x;
}
Eigen::MatrixXd temp;
temp = A.transpose()* A;
coeficients = (temp.inverse() * A.transpose() ) * anspose();
/
/coeficients = A.ldlt().solve(Yi_1); // A sym. p.s.d. #include <Eigen/Cholesky>
//coeficients = A.llt().solve(Yi_1); // A sym. p.d. #include <Eigen/Cholesky>
//coeficients = A.lu().solve(Yi_1); // Stable and fast. #include <Eigen/LU>
//coeficients = A.qr().solve(Yi_1); // No pivoting. #include <Eigen/QR>
//coeficients = A.svd().solve(Yi_1); // Stable, slowest. #include <Eigen/SVD>
}
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议