多点平面方程拟合c语言,关于最小二乘拟合空间平面的问题

#include "stdafx.h"

#include

#include

#define m_col 3

#define m_dim 3

void InvMatrix(const double M[][m_col], const int n, double invM[][m_col]);

void LinearFit(const double Coef[][m_dim], const int n, double *Para);

void main()

{

double A[4][3] = {0,1,7,1,0,6,2,1,11,-1,-1,-1};

double Para[3];

LinearFit(A, 4, Para);

printf("%lf %lf %lf",Para[0],Para[1],Para[2]);

}

void LinearFit(const double Coef[][m_dim], const int n, double *Para)

{

double (*A)[m_col] = new double[m_dim][m_col];

for(int i=0;i

{

for(int j=0;j

{

A[i][j] = 0;

for(int k=0;k

A[i][j]+= Coef[k][i]*Coef[k][j];

}

}

for(int i=0;i

{

A[i][m_dim-1] = A[m_dim-1][i] = 0;

for(int j=0;j

A[i][m_dim-1]+= Coef[j][i];

A[m_dim-1][i] = A[i][m_dim-1];

}

A[m_dim-1][m_dim-1] = n;

double *b = new double[m_dim];

for(int i=0;i

{

b[i] = 0;

for(int j=0;j

b[i]+= Coef[j][m_dim-1]*Coef[j][i];

}

b[m_dim-1] = 0;

for(int i=0;i

b[m_dim-1]+= Coef[i][m_dim-1];

double (*invA)[m_col] = new double[m_dim][m_col];

InvMatrix(A, m_dim, invA);

for(int i=0;i

{

Para[i] = 0;

for(int j=0;j

Para[i]+= invA[i][j]*b[j];

}

delete []A;

delete []invA;

delete b;

}

void InvMatrix(const double M[][m_col], const int n, double invM[][m_col])

{

if(n==1)

invM[0][0] = 1/M[0][0];

else

{

double cu, cd, normb;

double (*b)[m_col] = new double[n][m_col];

double (*invv)[m_col] = new double[n][m_col];

for(int j=0;j

{

double *schu = new double[j];

double *schd = new double[j];

if(j>0)

{

for(int k=0;k

{

schu[k] = schd[k] = 0;

for(int i=0;i

{

schu[k]+= b[i][k]*M[i][j];

schd[k]+= b[i][k]*b[i][k];

}

}

}

normb = 0;

for(int i=0;i

{

b[i][j] = M[i][j];

if(j>0)

{

for(int k=0;k

b[i][j]-= b[i][k]*schu[k]/schd[k];

}

normb+= b[i][j]*b[i][j];

}

normb = sqrt(normb);

for(int i=0;i

invv[j][i] = b[i][j]/normb;

delete schu;

delete schd;

}

double (*c)[m_col] = new double[n][m_col];

for(int i=0;i

{

for(int j=0;j<=i;j++)

{

cu = cd = 0;

if(j

{

for(int k=0;k

{

cu+= M[k][i]*b[k][j];

cd+= b[k][j]*b[k][j];

}

c[j][i] = cu/sqrt(cd);

}

else

{

for(int k=0;k

cd+= b[k][j]*b[k][j];

c[j][i] = sqrt(cd);

}

}

}

double (*invc)[m_col] = new double[n][m_col];

for(int j=0;j

{

for(int i=n-1;i>=0;i--)

{

if(i>j)

invc[i][j] = 0;

else if(i==j)

invc[i][j] = 1/c[i][j];

else

{

invc[i][j] = 0;

for (int k=i+1;k<=j;k++)

invc[i][j]-= c[i][k]*invc[k][j];

invc[i][j]/= c[i][i];

}

}

}

for(int i=0;i

{

for(int j=0;j

{

invM[i][j] = 0;

for(int k=0;k

invM[i][j]+= invc[i][k]*invv[k][j];

}

}

delete []b;

delete []c;

delete []invv;

delete []invc;

}

}

最近做项目恰好遇到这个问题，编了个小程序。

linerfit函数为矩阵法的多元线性最小二乘求解函数， 当m_dim设置为3时即可求平面的最小二乘，当然m_dim设置为2可求平面直线最小二乘。注意的是m_col应定义为大于等于m_dim。若目标函数为z=ax+by+c;那么coef传参格式为:

x1 y1 z1

x2 y2 z2

....

求解结果para = [a b c].

当然你也可以用InvMatrix子函数做其他用途，其为QR法快速求逆矩阵。

[本帖最后由 eastlife0108 于 2014-4-7 15:21 编辑]