opencv最小二乘拟合平面

//平面拟合（Ax+By+Cz+1=0）    
void cvFitPlane()    
{    
    double array[4][3],Y[3];//待拟合的点    
    ZeroMemory(array,sizeof(array));//分配内存空间    
    ZeroMemory(Y,sizeof(Y));    
    array[0][0]=1;    
    array[0][1]=-1;    
    array[0][2]=1;    
    array[1][0]=0;    
    array[1][1]=1;    
    array[1][2]=2;    
    array[2][0]=1;    
    array[2][1]=1;    
    array[2][2]=3;    
    array[3][0]=-1;    
    array[3][1]=-1;    
    array[3][2]=-1;    
    double *Matrix[3],*IMatrix[3];    
    for (int i = 0;i < 3;i++)    
    {    
        Matrix[i]  = new double[3];//矩阵与其逆矩阵相乘    
    }    
    for (int i = 0;i < 3;i++)    
    {    
        for (int j = 0;j < 3;j++)    
        {    
            *(Matrix[i] + j) = 0.0;    
        }    
    }    
    
    for (int j = 0;j < 3;j++)    
    {    
        fo