相机畸变矫正

这里引用了高翔博士打《SLAM 十四讲》，首先谢谢高博啦！

理论知识

说明：本文参考的应该是《SLAM 十四讲》第一版，在这里有个错误，后版中改了过来，这里提出，免得误导博友。公式5.11 - 5.14 中打Xcorrected和Ycorrected表示的是未矫正的坐标，也就是存在畸变打点的坐标。这里描述的是相机畸变的过程，将该过程取反即是去畸变过程）

畸变参数获取

1. 在标定相机时，即可获得径向畸变参数k，和切向畸变参数p。
2. 在式中出现的（r，theta）是由像素坐标转到极坐标得到。
3. fx和fy，cx和cy由相机内参得到。
4. x，y：理想图像中不存在畸变的点的坐标
5. Xcorrected和Ycorrected：畸变之后点的坐标

目的：求取x，y

矫正效果

说明：图片中打红色的圆是矫正之后打点，其相邻的角点是存在畸变的点。
（不要盯着我萌萌哒打师兄看哦！！！）

实现

    for(int i=0; i<size; i++)
    {
        Mx = P_Norm[i].x;
        My = P_Norm[i].y;
        rSquare = pow(Mx, 2) + pow(My, 2);
        A = (1 + strCamPara.k1 * rSquare + strCamPara.k2 * pow(rSquare, 2)
             + strCamPara.k3 * pow(rSquare, 3));
        B = 2*strCamPara.p1;
        C = 2*strCamPara.p2;
        D = strCamPara.p2*rSquare;
        E = strCamPara.p1*rSquare;

        a = (My - E)/(Mx - D);
        b = a*B + C;
        d = D - Mx;
        //solve the formula "b*x^2 + A*x + d = 0", then we will get 'x'
        e = pow(A, 2) - 4*b*d;

        //b=0 ?
        if(b==0)
        {
            //if b=0, then get the formula "A*x + d = 0", solve it and get 'x'
            x1 = d/A;
            y1 = a*x1;
            //cout<<i<<" e=0, and the point is "<<Point2f(x1,y1)<<endl;
            p_temp.push_back(Point2f(x1,y1));
        }
        else
        {
            if(e >= 0)
            {
                x1 = (-A + sqrt(e))*0.5/b;
                x2 = (-A - sqrt(e))*0.5/b;
                y1 = a*x1;
                y2 = a*x2;

        //give up these points Point2f(x1,y2),Point2f(x2,y1),Point2f(x2,y2)
        //cout<<i<<" point is "<<Point2f(x1,y1)<<endl;
        //cout<<i<<" point is "<<Point2f(x1,y2)<<endl;
        //cout<<i<<" point is "<<Point2f(x2,y2)<<endl;
        //cout<<i<<" point is "<<Point2f(x2,y2)<<endl;
                p_temp.push_back(Point2f(x1,y1));
            }
            else
            {
                cerr<<"e < 0, is "<<e<<endl;
                continue;
            }
        }
    }

完整的实现过程点击下载，内附存在畸变打照片及相机参数。

================分割线=====================

更正一下，SLAM十四讲的内容应该是没有问题的，而且很多资料上都和其是一致的
让我陷入怀疑书本有误的是上述代码（代码是按照公式推导出来的，没有问题）运行出来的结果，如上图，给人的感觉就是矫正，相反如果照着书本上编写相应的代码（这个比较容易，可以自己试试一下）并且运行之后的结果给人的感觉畸变过程，而且畸变更严重了，很奇怪的感觉