二次曲面的拟合-椭球曲面拟合

void  pbgGetCoeMatrxSur(zdouble_t coeMatrix[DN][DN], zdouble_t rowVector[DN], global_Trimap* pRgn)
{
    std::vectorfgR;
    std::vectorfgG;
    std::vectorfgB;

    int32_t strideS  = m_InputInfo.iStride;

    for (uint32_t i = 0; i < pRgn->pExtFgList.size(); i++)
    {
        blk_info_t *pBlkInfo = pRgn->pExtFgList[i];
        int32_t hcbUintType  = pBlkInfo->hcbUnitType;

        uint8_t* pSr  = pBlkInfo->pImgRgb[0];
        uint8_t* pSg  = pBlkInfo->pImgRgb[1];
        uint8_t* pSb  = pBlkInfo->pImgRgb[2];
        int32_t step  = 7;
        for (int32_t y = 0; y < BLK_SIZE - step; y += step)
        {
            for (int32_t x = 0; x < BLK_SIZE -step; x += step)
            {
                int32_t valueR = pSr[x];
                int32_t valueG = pSg[x];
                int32_t valueB = pSb[x];

                fgR.push_back(valueR);
                fgG.push_back(valueG);
                fgB.push_back(valueB);
            }
            pSr     += strideS * step;
            pSg     += strideS * step;
            pSb     += strideS * step;
        }
    }

    int32_t dataSize = fgR.size();
    for (int32_t i = 0; i < dataSize; i ++)
    {
        int32_t rValue = fgR[i];
        int32_t gValue = fgG[i];
        int32_t bValue = fgB[i];
        int32_t rValue2 = rValue  * rValue;
        int32_t gValue2 = gValue  * gValue;
        int32_t rgValue = rValue  * gValue;
        int32_t rValue3 = rValue2 * rValue;
        int32_t gValue3 = gValue2 * gValue;
        int32_t rValue4 = rValue3 * rValue;
        int32_t gValue4 = gValue3 * gValue;

        coeMatrix[0][0] += 1;
        coeMatrix[0][1] += rValue;
        coeMatrix[0][2] += gValue;
        coeMatrix[0][3] += rValue2;
        coeMatrix[0][4] += rgValue;
        coeMatrix[0][5] += gValue2;

        coeMatrix[1][1] += rValue2;
        coeMatrix[1][2] += rgValue;
        coeMatrix[1][3] += rValue3;
        coeMatrix[1][4] += (rValue2 * gValue);
        coeMatrix[1][5] += (rValue  * gValue2);

        coeMatrix[2][2] += gValue2;
        coeMatrix[2][3] += (rValue2 * gValue);
        coeMatrix[2][4] += (rValue  * gValue2);
        coeMatrix[2][5] += gValue3;

        coeMatrix[3][3] += rValue4;
        coeMatrix[3][4] += (rValue3 * gValue);
        coeMatrix[3][5] += (rValue2 * gValue2);

        coeMatrix[4][4] += (rValue2 * gValue2);
        coeMatrix[4][5] += (rValue  * gValue3) ;

        coeMatrix[5][5] += gValue4;

        rowVector[0] += bValue;
        rowVector[1] += bValue * rValue;
        rowVector[2] += bValue * gValue;
        rowVector[3] += bValue * rValue2;
        rowVector[4] += bValue * rgValue;
        rowVector[5] += bValue * gValue2;
    }
    // symmetric matrix
    for (int32_t rows = 0; rows < 6; rows ++)
    {
        for (int32_t cols = 0; cols < rows; cols ++)
        {
            coeMatrix[rows][cols] = coeMatrix[cols][rows];
        }
    }
}

void  pbgGetCoeMatrxSurNoneDir(zdouble_t coeMatrix[DN][DN], zdouble_t rowVector[DN], global_Trimap* pRgn)
{
    std::vectorfgR;
    std::vectorfgG;
    std::vectorfgB;

    int32_t strideS  = m_InputInfo.iStride;

    for (uint32_t i = 0; i < pRgn->pExtFgList.size(); i++)
    {
        blk_info_t *pBlkInfo = pRgn->pExtFgList[i];
        int32_t hcbUintType  = pBlkInfo->hcbUnitType;

        uint8_t* pSr  = pBlkInfo->pImgRgb[0];
        uint8_t* pSg  = pBlkInfo->pImgRgb[1];
        uint8_t* pSb  = pBlkInfo->pImgRgb[2];
        int32_t step  = 7;
        for (int32_t y = 0; y < BLK_SIZE - step; y += step)
        {
            for (int32_t x = 0; x < BLK_SIZE -step; x += step)
            {
                int32_t valueR = pSr[x];
                int32_t valueG = pSg[x];
                int32_t valueB = pSb[x];

                fgR.push_back(valueR);
                fgG.push_back(valueG);
                fgB.push_back(valueB);
            }
            pSr     += strideS * step;
            pSg     += strideS * step;
            pSb     += strideS * step;
        }
    }

    int32_t dataSize = fgR.size();
    for (int32_t i = 0; i < dataSize; i ++)
    {
        int32_t rValue = fgR[i];
        int32_t gValue = fgG[i];
        int32_t bValue = fgB[i];
        int32_t rValue2 = rValue  * rValue;
        int32_t rValue3 = rValue2 * rValue;
        int32_t rValue4 = rValue3 * rValue;

        int32_t gValue2 = gValue  * gValue;
        int32_t gValue3 = gValue2 * gValue;
        int32_t gValue4 = gValue3 * gValue;

        int32_t bValue2 = bValue  * bValue;
        int32_t bValue3 = bValue2 * bValue;
        int32_t bValue4 = bValue3 * bValue;

        coeMatrix[0][0] += gValue4;
        coeMatrix[0][1] += gValue2 * bValue2;
        coeMatrix[0][2] += rValue  * gValue2;
        coeMatrix[0][3] += gValue3;
        coeMatrix[0][4] += gValue2 * bValue;
        coeMatrix[0][5] += gValue2;

        coeMatrix[1][1] += bValue4;
        coeMatrix[1][2] += rValue * bValue2;
        coeMatrix[1][3] += gValue * bValue2;
        coeMatrix[1][4] += bValue3;
        coeMatrix[1][5] += bValue2;

        coeMatrix[2][2] += rValue2;
        coeMatrix[2][3] += rValue * gValue;
        coeMatrix[2][4] += rValue * bValue;
        coeMatrix[2][5] += rValue;

        coeMatrix[3][3] += gValue2;
        coeMatrix[3][4] += gValue * bValue;
        coeMatrix[3][5] += gValue;

        coeMatrix[4][4] += bValue2;
        coeMatrix[4][5] += bValue;

        coeMatrix[5][5] += 1;

        rowVector[0] -= rValue2 * gValue2;
        rowVector[1] -= rValue2 * bValue2;
        rowVector[2] -= rValue3;
        rowVector[3] -= rValue2 * gValue;
        rowVector[4] -= rValue2 * bValue;
        rowVector[5] -= rValue2;
    }
    // symmetric matrix
    for (int32_t rows = 0; rows < 6; rows ++)
    {
        for (int32_t cols = 0; cols < rows; cols ++)
        {
            coeMatrix[rows][cols] = coeMatrix[cols][rows];
        }
    }
}