Eigen: 二维高斯曲面拟合法求取光斑中心及算法的C++实现

（1）二维高斯去曲面拟合推导一个二维高斯方程可以写成如下形式：


其中，G为高斯分布的幅值，,为x,y方向上的标准差，对式（1）两边取对数，并展开平方项，整理后为：


假如参与拟合的数据点有N个，则将这个N个数据点写成矩阵的形式为：A = B C，其中：A为N*1的向量，其元素为：

B为N*5的矩阵：



C为一个由高斯参数组成的向量：



（2）求解二维高斯曲线拟合N个数据点误差的列向量为：E=A-BC，用最小二乘法拟合，使其N个数据点的均方差最小，即：


在图像数据处理时，数据量比较大，为减小计算量，将矩阵B进行QR分解，即：B=QR，分解后Q为一个N*N的正交矩阵，R为一个N*5的上三角矩阵，对E=A-BC进行如下推导：



由于Q为正交矩阵，可以得到：



令：

其中：
S为一个5维列向量；T为一个N-5维列向量；R1为一个5*5的上三角方阵，则


上式中，当S = R1C时取得最小值，因此只需解出：



即可求出：



中的

这些参数，这里先给出：


这里：



（3）C++代码实现，算法的实现过程中由于涉及大量的矩阵运算，所以采用了第三方的开源矩阵算法Eigen，这里真正用于高斯拟合的函数是bool GetCentrePoint(float& x0,float& y0)
关于Eigen的用法请参考：http://blog.csdn.net/hjx_1000/article/details/8490941

#include "stdafx.h"  
#include "GaussAlgorithm.h"  
  
VectorXf m_Vector_A;  
MatrixXf m_matrix_B;  
int m_iN =-1;  
  
bool InitData(int pSrc[100][100], int iWidth, int iHeight)  
{  
    if (NULL == pSrc || iWidth <=0 || iHeight <= 0)  
        return false;  
    m_iN = iWidth*iHeight;  
    VectorXf tmp_A(m_iN);  
    MatrixXf tmp_B(m_iN, 5);  
    int i =0, j=0, iPos =0;  
    while(i<iWidth)  
    {  
         j=0;  
        while(j<iHeight)  
        {  
            tmp_A(iPos) = pSrc[i][j] * log((float)pSrc[i][j]);  
  
            tmp_B(iPos,0) = pSrc[i][j] ;  
            tmp_B(iPos,1) = pSrc[i][j] * i;  
            tmp_B(iPos,2) = pSrc[i][j] * j;  
            tmp_B(iPos,3) = pSrc[i][j] * i * i;  
            tmp_B(iPos,4) = pSrc[i][j] * j * j;  
            ++iPos;  
            ++j;  
        }  
        ++i;  
    }  
    m_Vector_A = tmp_A;  
    m_matrix_B = tmp_B;  
  
}  
bool GetCentrePoint(float& x0,float& y0)  
{  
    if (m_iN<=0)  
        return false;  
    //QR分解  
    HouseholderQR<MatrixXf> qr;  
    qr.compute(m_matrix_B);  
    MatrixXf R = qr.matrixQR().triangularView<Upper>();  
    MatrixXf Q =  qr.householderQ();  
  
    //块操作，获取向量或矩阵的局部  
    VectorXf S;  
    S = (Q.transpose()* m_Vector_A).head(5);  
    MatrixXf R1;  
    R1 = R.block(0,0,5,5);  
  
    VectorXf C;  
    C = R1.inverse() * S;  
  
    x0 = -0.5 * C[1] / C[3];  
    y0 = -0.5 * C[2] / C[4];  
  
    return true;  
}

其中：函数 bool InitData(int pSrc[100][100], int iWidth, int iHeight)主要用于将数组转换成相应的矩阵，因为我的所有数据都在一个整形的二维数组中存着，所以需要做这种转换。函数bool GetCentrePoint(float& x0,float& y0)主要用于对数据点进行二维高斯曲面拟合，并返回拟合的光点中心。
http://blog.csdn.net/houjixin/article/details/8490653