空间直线的最小二乘拟合

转自：http://blog.chinaunix.net/uid-25296429-id-4901934.html

注意：用(y-y0)/n = (z-z0)/m1 = x/1推导得出的m1理想状态下应为m的倒数，但实际情况并非如此。

可推算m*m1 = sqr(p(x,z)) = sqr(conv(x, z))/(var(x)*var(z)), 也就是x和z线性相关系数的平方。

理论推导过程：

求矩阵即可得到m,n,x0,y0，直线方程的各个参数即为所得。
#include 
#include 
#include 
#include 
#define MAX 10
#define PtSum 38
#define MTX_AMT 100


void Inverse(double *matrix1[],double *matrix2[],int n,double d);
double Determinant(double* matrix[],int n);
double AlCo(double* matrix[],int jie,int row,int column);
double Cofactor(double* matrix[],int jie,int row,int column);
void Empty(double *matrix[],int row,int column);


int main(int argc, CHAR* argv[])
{
    double array[MTX_AMT][3],*Matrix[2],*IMatrix[2];//允许有MTX_AMT个点
    //设定待拟合的直线(x -1)/3 = (y - 2)/2 = z/1
    //x = 3z + 1
    //y = 2z + 2
    for (int i = 0; i < PtSum;i++)
    {
        array[i][2] = (double)i; 
        array[i][0] = 3*array[i][2] + 1 + rand()/RAND_MAX;  //引入噪声
        array[i][1] = 2*array[i][2] + 2 + rand()/RAND_MAX;  //引入噪声
    }
    double X0,Y0,m,n;//这是待拟合的四个参数，已知分别对应：1、2、3、2现在我们来拟合
    for ( i = 0;i < 2;i++)
    {
        Matrix[i] =  new double[2];
        IMatrix[i] = new double[2];
    }
    Empty(Matrix,2,2);//将矩阵的前两行两列置位0


    for (int j = 0;j < PtSum;j++)//
    {
        *(Matrix[0] + 0) += array[j][2]*array[j][2];//Matrix[0][0]=array[0][2]*array[0][2]=z*z
        *(Matrix[0] + 1) += array[j][2];//Matrix[0][1]=array[0][2]=z
    }
    *(Matrix[1] + 0) = *(Matrix[0] + 1);//Matrix[1][0]=z;
    *(Matrix[1] + 1) = 1.0*PtSum;//Matrix[1][1]=n;


//下面两步是求Matrix的逆矩阵并存到IMatrix中
//A^(-1)=(1/|A|)×A* ，其中A^(-1)表示矩阵A的逆矩阵，其中|A|为矩阵A的行列式，A*为矩阵A的伴随矩阵。
    double d = Determinant(Matrix,2);
    Inverse(Matrix,IMatrix,2,d);//反转存到IMatrix中


    Empty(Matrix,2,2);
    for ( i = 0;i < PtSum;i++)
    {
        *(Matrix[0] + 0) += array[i][0]*array[i][2];//xi*zi的和
        *(Matrix[0] + 1) += array[i][0];//xi
        *(Matrix[1] + 0) += array[i][1]*array[i][2];//yi*zi的和
        *(Matrix[1] + 1) += array[i][1];//yi
    }
    m = *(Matrix[0] + 0)**(IMatrix[0] + 0) + *(Matrix[0] + 1)**(IMatrix[1] + 0);
    X0= *(Matrix[0] + 0)**(IMatrix[0] + 1) + *(Matrix[0] + 1)**(IMatrix[1] + 1);
    n = *(Matrix[1] + 0)**(IMatrix[0] + 0) + *(Matrix[1] + 1)**(IMatrix[1] + 0);
    Y0= *(Matrix[1] + 0)**(IMatrix[0] + 1) + *(Matrix[1] + 1)**(IMatrix[1] + 1);
    printf("%5.2f\t%5.2f\t%5.2f\t%5.2f\t",X0,Y0,m,n);
    for ( i = 0;i < 2;i++)
    {
        delete[] Matrix[i];
        delete[] IMatrix[i];
    }
    getchar();
    return 0;
}


void Empty(double *matrix[],int row,int column)//初始化
{
    for (int i = 0;i < row;i++)
    {
        for (int j = 0;j < column;j++)
        {
            *(matrix[i] + j) = 0.0;
        }
    }
}


void Inverse(double *matrix1[],double *matrix2[],int n,double d) //
{ 
    int i,j; 
    for(i=0;i<n;i++) 
         matrix2[i]=(double *)malloc(n*sizeof(double)); //给IMatrix一维指针分配空间
    for(i=0;i<n;i++) 
         for(j=0;j<n;j++) 
             *(matrix2[j]+i)=(AlCo(matrix1,n,i,j)/d); //给IMatrix二维数据分配空间
} 


double Determinant(double* matrix[],int n) // 计算行列式
{  
    double result=0,temp;  
    int i;  
    if(n == 1)  
        result=(*matrix[0]);  
    else  
    {  
        for(i=0;i<n;i++)  
         {  
            temp=AlCo(matrix,n,n-1,i);  
            result+=(*(matrix[n-1]+i))*temp;  
        }  
    }  
    return result;  
}  


double AlCo(double* matrix[],int jie,int row,int column)  //
{  
    double result; 
    if((row+column)%2 == 0) //如果不是偶数倍
        result = Cofactor(matrix,jie,row,column);  
    else result=(-1)*Cofactor(matrix,jie,row,column); 
    return result;  
}  


double Cofactor(double* matrix[],int jie,int row,int column)  
{  
    double result;  
    int i,j;  
    double* smallmatr[MAX-1];  
    for(i=0;i<jie-1;i++)  
         smallmatr[i]= new double[jie - 1];//n*n矩阵
    for(i=0;i<row;i++)  
         for(j=0;j<column;j++)  
             *(smallmatr[i]+j)=*(matrix[i]+j);  //给smallmatr赋值
    for(i=row;i<jie-1;i++)  
         for(j=0;j<column;j++)  
             *(smallmatr[i]+j)=*(matrix[i+1]+j);  
    for(i=0;i<row;i++)  
         for(j=column;j<jie-1;j++)  
             *(smallmatr[i]+j)=*(matrix[i]+j+1);  
    for(i=row;i<jie-1;i++)  
         for(j=column;j<jie-1;j++)  
             *(smallmatr[i]+j)=*(matrix[i+1]+j+1);  
    result = Determinant(smallmatr,jie-1); 
    for(i=0;i<jie-1;i++)
         delete[] smallmatr[i];
    return result;   
}