针孔相机模型及畸变

针孔相机模型：

简化模型：

空间点$P$坐标$left[X,Y,Z ight]^{T}$，落在物理成像平面为$P^{'}$坐标$left[X^{'},Y^{'},Z^{'} ight]^{T}$

$frac{Z}{f}=-frac{X}{X^{'}}=-frac{Y}{Y^{'}}$ ($Z^{'}=f$)

尽管从物理原理来说，小孔成像应该是倒立的，由于对图像作了预处理，不加限制地称后一种情况$frac{Z}{f}=frac{X}{X^{'}}=frac{Y}{Y^{'}}$为针孔模型。

由上可知：

$X^{'}=ffrac{X}{Z}$

$Y^{'}=ffrac{Y}{Z}$

$P^{'}$点坐标$left[X^{'},Y^{'},Z^{'} ight]^{T}$对应像素坐标为$left[u,v ight]^{T}$

像素坐标系：原点位于左上方，像素坐标系与成像平面之间，相差了一个缩放和原点平移。u轴缩放为$alpha$，平移为$c_{{x}}$，v轴缩放为$eta$,平移为$c_{{y}}$。

$u = alpha{X^{'}}+c_{x}$

$v = eta{Y^{'}}+c_{y}$

将成像平面缩放至像素平面。$O$为成像平面原点，$O^{'}$为像素坐标原点。

因为$X^{'}=ffrac{X}{Z}$，$Y^{'}=ffrac{Y}{Z}$

$u = alphacdot{f}frac{X}{Z}+c_{x}$

$v = etacdot{f}frac{Y}{Z}+c_{y}$

把$alphacdot{f}$合并成$f_{x}$，$etacdot{f}$合并成$f_{y}$

得

$u = f_{x}frac{X}{Z}+c_{x}$

$v = f_{y}frac{Y}{Z}+c_{y}$

用到齐次坐标：

$egin{bmatrix} u \ v \ 1 end{bmatrix} = frac{1}{Z} egin{bmatrix} f_{x} & 0 & c_{x} \ 0 & f_{y} & c_{y} \ 0 & 0 & 1 end{bmatrix} egin{bmatrix} X \ Y \ Z end{bmatrix} riangleq frac{1}{Z}Kcdot{P}$

$P = left[X,Y,Z ight]^{T}$为空间坐标，$K$为内参矩阵。

一般将$Z$挪到右边，$Zegin{bmatrix} u \ v \ 1 end{bmatrix} =  egin{bmatrix} f_{x} & 0 & c_{x} \ 0 & f_{y} & c_{y} \ 0 & 0 & 1 end{bmatrix} egin{bmatrix} X \ Y \ Z end{bmatrix} riangleq Kcdot{P}$

$P_{uv}$为像素坐标

$Zcdot{P_{uv}} = Zegin{bmatrix} u \ v \ 1 end{bmatrix} = Kcdot left( Rcdot P_{W} + t ight) = Kcdot T cdot P_{W}$

$P_W$为$P$点在世界坐标系下的坐标。

畸变：

由上可知，理想坐标点$left(u,v ight)$经畸变后所在位置为$left(u_d,v_d ight)$，由此可根据$u_d,v_d$在畸变图像中找到颜色放到正确的位置$left(u,v ight)$。

对应程序

4 
#include <opencv2/opencv.hpp>
#include <string>
#include <cmath>

using namespace std;

string image_file = "./test.png";   // 请确保路径正确

int main(int argc, char **argv) {

    // 本程序需要你自己实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。
    // 畸变参数
    double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
    // 内参
    double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;

    cv::Mat image = cv::imread(image_file,0);   // 图像是灰度图，CV_8UC1
    int rows = image.rows, cols = image.cols;
    cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 去畸变以后的图

    // 计算去畸变后图像的内容
    for (int v = 0; v < rows; v++)
        for (int u = 0; u < cols; u++) 
        {

            double u_distorted = 0, v_distorted = 0;
            double m=0,n=0,a=0,b=0,r=0;
            // TODO 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted) (~6 lines)

            // start your code here
        m = (u-cx)/fx;//将像素点位置转为归一化坐标
            n = (v-cy)/fy;
            r = sqrt(pow(m,2)+pow(n,2));

            a = m*(1 + k1*pow(r,2) + k2*pow(r,4) ) + 2*p1*m*n + p2*(pow(r,2) + 2*pow(m,2));//矫正后
            b = n*(1 + k1*pow(r,2) + k2*pow(r,4) ) + p1*(pow(r,2) + 2*pow(n,2)) + 2*p2*m*n;
            u_distorted = fx*a + cx;//矫正后代回
            v_distorted = fy*b + cy;
            // end your code here

            // 赋值 (最近邻插值)
            if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) 
            {
                image_undistort.at<uchar>(v, u) = image.at<uchar>( (int)v_distorted, (int)u_distorted );
                //移动像素的过程，将畸变图像的像素进行移动,把灰度值放到正确的位置
            } 
            else 
            {
                image_undistort.at<uchar>(v, u) = 255;
            }
        }

    // 画图去畸变后图像
    cv::imshow("image undistorted", image_undistort);
    cv::imwrite("undistorted.png", image_undistort);
    cv::waitKey();

    return 0;
}