针孔相机模型：
简化模型：
空间点$P$坐标$\left[X,Y,Z\right]^{T}$，落在物理成像平面为$P^{'}$坐标$\left[X^{'},Y^{'},Z^{'}\right]^{T}$
$\frac{Z}{f}=-\frac{X}{X^{'}}=-\frac{Y}{Y^{'}}$ ($Z^{'}=f$)
尽管从物理原理来说，小孔成像应该是倒立的，由于对图像作了预处理，不加限制地称后一种情况$\frac{Z}{f}=\frac{X}{X^{'}}=\frac{Y}{Y^{'}}$为针孔模型。
由上可知：
$X^{'}=f\frac{X}{Z}$
$Y^{'}=f\frac{Y}{Z}$
$P^{'}$点坐标$\left[X^{'},Y^{'},Z^{'}\right]^{T}$对应像素坐标为$\left[u,v\right]^{T}$
像素坐标系：原点位于左上方，像素坐标系与成像平面之间，相差了一个缩放和原点平移。u轴缩放为$\alpha$，平移为$c_{{x}}$，v轴缩放为$\beta$,平移为$c_{{y}}$。
$u = \alpha{X^{'}}+c_{x}$
$v = \beta{Y^{'}}+c_{y}$
将成像平面缩放至像素平面。$O$为成像平面原点，$O^{'}$为像素坐标原点。
因为$X^{'}=f\frac{X}{Z}$，$Y^{'}=f\frac{Y}{Z}$
$u = \alpha\cdot{f}\frac{X}{Z}+c_{x}$
$v = \beta\cdot{f}\frac{Y}{Z}+c_{y}$
把$\alpha\cdot{f}$合并成$f_{x}$，$\beta\cdot{f}$合并成$f_{y}$
得
$u = f_{x}\frac{X}{Z}+c_{x}$
$v = f_{y}\frac{Y}{Z}+c_{y}$
用到齐次坐标：
$\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \frac{1}{Z} \begin{bmatrix} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} \triangleq \frac{1}{Z}K\cdot{P}$
$P = \left[X,Y,Z\right]^{T}$为空间坐标，$K$为内参矩阵。
一般将$Z$挪到右边，$Z\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \begin{bmatrix} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix} \triangleq K\cdot{P}$
$P_{uv}$为像素坐标
$Z\cdot{P_{uv}} = Z\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = K\cdot \left( R\cdot P_{W} + t \right) = K\cdot T \cdot P_{W}$
$P_W$为$P$点在世界坐标系下的坐标。
畸变：
由上可知，理想坐标点$\left(u,v\right)$经畸变后所在位置为$\left(u_d,v_d\right)$，由此可根据$u_d,v_d$在畸变图像中找到颜色放到正确的位置$\left(u,v\right)$。
对应程序
1 // 2 // Created by 高翔 on 2017/12/15. 3 // 4 5 #include <opencv2/opencv.hpp> 6 #include <string> 7 #include <cmath> 8 9 using namespace std; 10 11 string image_file = "./test.png"; // 请确保路径正确 12 13 int main(int argc, char **argv) { 14 15 // 本程序需要你自己实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。 16 // 畸变参数 17 double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05; 18 // 内参 19 double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375; 20 21 cv::Mat image = cv::imread(image_file,0); // 图像是灰度图，CV_8UC1 22 int rows = image.rows, cols = image.cols; 23 cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1); // 去畸变以后的图 24 25 // 计算去畸变后图像的内容 26 for (int v = 0; v < rows; v++) 27 for (int u = 0; u < cols; u++) 28 { 29 30 double u_distorted = 0, v_distorted = 0; 31 double m=0,n=0,a=0,b=0,r=0; 32 // TODO 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted) (~6 lines) 33 34 // start your code here 35 m = (u-cx)/fx;//将像素点位置转为归一化坐标 36 n = (v-cy)/fy; 37 r = sqrt(pow(m,2)+pow(n,2)); 38 39 a = m*(1 + k1*pow(r,2) + k2*pow(r,4) ) + 2*p1*m*n + p2*(pow(r,2) + 2*pow(m,2));//矫正后 40 b = n*(1 + k1*pow(r,2) + k2*pow(r,4) ) + p1*(pow(r,2) + 2*pow(n,2)) + 2*p2*m*n; 41 u_distorted = fx*a + cx;//矫正后代回 42 v_distorted = fy*b + cy; 43 // end your code here 44 45 // 赋值 (最近邻插值) 46 if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) 47 { 48 image_undistort.at<uchar>(v, u) = image.at<uchar>( (int)v_distorted, (int)u_distorted ); 49 //移动像素的过程，将畸变图像的像素进行移动,把灰度值放到正确的位置 50 } 51 else 52 { 53 image_undistort.at<uchar>(v, u) = 255; 54 } 55 } 56 57 // 画图去畸变后图像 58 cv::imshow("image undistorted", image_undistort); 59 cv::imwrite("undistorted.png", image_undistort); 60 cv::waitKey(); 61 62 return 0; 63 }