光斑亚像素处理

#pragma once
#include
typedef unsigned char uchar;
#include

class SubPixCenter
{
    int m_iN = -1;
    Eigen::VectorXf m_Vector_A;
    Eigen::MatrixXf m_matrix_B;
    bool InitData(cv::Mat& roi, const  double THR);
    bool GetCentrePoint(double& x0, double& y0);
public:
    SubPixCenter();
    ~SubPixCenter();

    bool fitGaussCenter(cv::Mat& roi, cv::Point2d& center, const double THR);

    void test();

    double stdx, stdy;
};

 

 

 

#include "SubPixCenter.h"
using namespace Eigen;

bool SubPixCenter::InitData(cv::Mat& pSrc, const double THR)
{
    if (pSrc.empty())
        return false;
    int iWidth = pSrc.cols;
    int iHeight = pSrc.rows;

    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)
        {
            float val = pSrc.at(j, i)+0.0001;
        
            tmp_A(iPos) = val * log(val);

            tmp_B(iPos, 0) = val;
            tmp_B(iPos, 1) = val * i;
            tmp_B(iPos, 2) = val * j;
            tmp_B(iPos, 3) = val * i * i;
            tmp_B(iPos, 4) = val * j * j;
            ++iPos;
            ++j;
        }
        ++i;
    }
    m_Vector_A = tmp_A;
    m_matrix_B = tmp_B;

    return true;
}

bool SubPixCenter::GetCentrePoint(double& x0, double& y0)
{
    if (m_iN <= 0)
        return false;
    //QR分解
    HouseholderQR qr;
    qr.compute(m_matrix_B);
    MatrixXf R = qr.matrixQR().triangularView();
    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];

    stdx =  -0.5/ C[3];
    stdy = -0.5/ C[4];

    return true;
}

bool SubPixCenter::fitGaussCenter(cv::Mat& roi, cv::Point2d& center,const double THR)
{
    if (!InitData(roi, THR))
        return false;
    return GetCentrePoint(center.x, center.y);
}

std::vector findpts(cv::Mat Img,float THR) {
    cv::Mat a = Img > THR;
    cv::Mat imgbk = Img.clone();
    cv::Mat bwbk = a.clone();
    std::vector > contours;
    cv::findContours(a, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);

    std::vector center_pts(contours.size());
    for (int j = 0; j < contours.size(); ++j) {
        //center_pts[j] = get_shape_center_from_point2i_s(contours[j]);
        cv::Point2f centerPt;
        float radius = 0;
        cv::minEnclosingCircle(contours[j], centerPt, radius);
        center_pts[j] = cv::Point2d(centerPt.x, centerPt.y);
        const int w_i = radius;
        cv::Rect bdrect(center_pts[j].x - w_i, center_pts[j].y - w_i, w_i * 2, w_i * 2);
        //center_pts[j] = get_shape_center_from_point2i_d(imgbk, bwbk, bdrect , THR);
    }
    return center_pts;
}

void SubPixCenter::test()
{
#define SQR(x) ((x)*(x))

    float x = 20;
    float y = 40;
    float thx = 2*SQR(2);
    float thy = 2*SQR(3);
    cv::Mat im = cv::Mat::zeros(100, 100, CV_8UC1);

    cv::RNG rng;
    rng.fill(im, cv::RNG::UNIFORM, 1, 40,true);

    for (int i = 0; i < im.cols; ++i) {
        for (int j = 0; j < im.rows; ++j) {
            float vv = im.at(j, i) + (float)255 * expf(-SQR(i - x) / thx - SQR(j - y) / thy);
            if (vv > 255)
                vv = 255;
            if (vv < 70)
                vv = 0;

            im.at(j, i) = vv;
            
        }
    }

    auto pts = findpts(im, 70);
    std::cout << "pts:" << pts << "\n";

    cv::Point2d cet;
    fitGaussCenter(im, cet,1);

    std::cout << "stdx=" << this->stdx << ",stdy=" << stdy << "\n";

    std::cout << "x = " << x << ",y =" << y << "\n";
    std::cout << "cet = " << cet;
    cv::imshow("im", im);
    cv::imwrite("img.jpg", im);

    cv::waitKey(0);
    //system("pause");

}

SubPixCenter::SubPixCenter()
{
}

SubPixCenter::~SubPixCenter()
{
}