去畸变理论
（具体内容见视觉slam14讲P97，且由于空间受限，本文推导均不放图片，有需要去查看电子书或实体书）
首先，把一会要用到的量先列出来
- 现实世界中 P P P点( P P P在相机坐标系下的)坐标 [ X , Y , Z ] T [X, Y, Z]^T [X,Y,Z]T
- 落在物理成像平面
O
′
−
x
′
−
y
′
O' - x' - y'
O′−x′−y′ 上，成像点
P
′
P'
P′为
[
X
′
,
Y
′
,
Z
′
]
T
[X', Y', Z']^T
[X′,Y′,Z′]T
- 注意：从图像中提取出来的点的坐标是像素坐标，而这个物理成像平面的坐标 X ′ = X Z X'=\frac{X}{Z} X′=ZX
- 去畸变时公式操控的点也是在成像平面上的点，而不是像素平面
- 相机的世界坐标 P w P_w Pw
- 像素坐标 P u v = [ u , v ] T P_{uv}=[u, v]^T Puv=[u,v]T
- 透镜焦距为 f f f
其次，理论推导
通过小孔成像的模型，我们有如下公式
Z f = − X X ′ = − Y Y ′ (5.1) \frac{Z}{f} = -\frac{X}{X'} = -\frac{Y}{Y'}\tag{5.1} fZ=−X′X=−Y′Y(5.1)
放到归一化平面上，就得到了
Z
f
=
X
X
′
=
Y
Y
′
(5.2)
\frac{Z}{f} = \frac{X}{X'} = \frac{Y}{Y'}\tag{5.2}
fZ=X′X=Y′Y(5.2)
化简，有
X
′
=
f
X
Z
Y
′
=
f
Y
Z
(5.3)
X' = f\frac{X}{Z} \quad Y' = f\frac{Y}{Z}\tag{5.3}
X′=fZXY′=fZY(5.3)
由于像素平面和图像坐标系不完全重合，因此
u
=
α
X
′
+
c
x
v
=
β
Y
′
+
c
y
(5.4)
\begin{aligned} u &= \alpha X' + c_x \\ v &= \beta Y' + c_y \\ \end{aligned}\tag{5.4}
uv=αX′+cx=βY′+cy(5.4)
把
(
5.3
)
(5.3)
(5.3)带入，并合并一下，有
{
u
=
f
x
X
Z
+
c
x
v
=
f
y
Y
Z
+
c
y
(5.5)
\begin{cases} u = f_x \frac{X}{Z} + c_x \\ v = f_y \frac{Y}{Z} + c_y \\ \end{cases} \tag{5.5}
{u=fxZX+cxv=fyZY+cy(5.5)
写成矩阵的形式，有
Z
(
u
v
1
)
=
(
f
x
0
c
x
0
f
y
c
y
0
0
1
)
(
X
Y
Z
)
(5.6)
Z \begin{pmatrix} u \\ v \\ 1 \end{pmatrix} = \begin{pmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} X \\ Y \\ Z \end{pmatrix}\tag{5.6}
Z
uv1
=
fx000fy0cxcy1
XYZ
(5.6)
其中
(
u
v
1
)
=
1
Z
K
P
(5.6)
\begin{pmatrix} u \\ v \\ 1 \end{pmatrix}= \frac{1}{Z} K P \tag{5.6}
uv1
=Z1KP(5.6)
对于
(
5
,
6
)
(5,6)
(5,6)这个公式，我们需要特别注意，并且以后将他作为结论来记住。
P
P
P是现实世界的坐标，而
P
Z
\frac{P}{Z}
ZP就代表归一化平面上的点。所以这个结论我们记为，归一化平面上的点乘
K
K
K这个矩阵，就变成了像素平面上的的点，相反的，像素平面上的点乘
K
−
1
K^{-1}
K−1就变成了归一化平面上的点
按照传统习惯，将 Z 移到左侧：
Z ( u v 1 ) = ( f x 0 c x 0 f y c y 0 0 1 ) ( X Y Z ) = K P (5.7) Z\begin{pmatrix} u \\ v \\ 1 \end{pmatrix} = \begin{pmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{pmatrix} \begin{pmatrix} X \\ Y \\ Z \end{pmatrix} = K P \tag{5.7} Z uv1 = fx000fy0cxcy1 XYZ =KP(5.7)
去畸变步骤
首先，我们肯定有一张畸变的图像
上边的像素点我们都已知，为 p u v p_{uv} puv
我们再来看 ( 5.6 ) 式 (5.6)式 (5.6)式
中间的 Z Z Z实质上就是一个常数，我们可以把它忽略不看，那么这个已知的 P u v P_{uv} Puv其实就是 P P P乘上一个矩阵 K K K，那么这个时候我们对 P u v P_{uv} Puv乘一个 K − 1 K^{-1} K−1就得到了 P P P，也就是归一化平面上每个点的坐标
利用去畸变公式
公式如下
x
d
i
s
t
o
r
t
e
d
=
x
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
+
2
p
1
x
y
+
p
2
(
r
2
+
2
x
2
)
x_{distorted} = x(1 + k_1r^2 + k_2r^4 + k_3r^6) + 2p_1xy + p_2(r^2 + 2x^2)
xdistorted=x(1+k1r2+k2r4+k3r6)+2p1xy+p2(r2+2x2)
y
d
i
s
t
o
r
t
e
d
=
y
(
1
+
k
1
r
2
+
k
2
r
4
+
k
3
r
6
)
+
p
1
(
r
2
+
2
y
2
)
+
2
p
2
x
y
y_{distorted} = y(1 + k_1r^2 + k_2r^4 + k_3r^6) + p_1(r^2 + 2y^2) + 2p_2xy
ydistorted=y(1+k1r2+k2r4+k3r6)+p1(r2+2y2)+2p2xy
我们就可以在原图像上利用这个公式对原图像进行变换，从根源上把这个图像”改对“，这样子，再对修改后的图片乘
K
K
K，就得到了去畸变后的
P
u
v
P_{uv}
Puv
公式解读
原码
#include <opencv2/opencv.hpp>
#include <string>
using namespace std;
string image_file = "./distorted.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++) {
// 按照公式，计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted)
double x = (u - cx) / fx, y = (v - cy) / fy;
double r = sqrt(x * x + y * y);
double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);
double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;
double u_distorted = fx x_distorted + cx;
double v_distorted = fy * y_distorted + cy;
// 赋值 (最近邻插值)
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) = 0;
}
}
}
// 画图去畸变后图像
cv::imshow("distorted", image);
cv::imshow("undistorted", image_undistort);
cv::waitKey();
return 0;
}
解读
- cv::Mat
cv::Mat是 OpenCV（开放源代码计算机视觉库）中的一个类，用于表示图像或矩阵数据。cv::Mat类提供了许多功能，例如创建、加载、保存和处理图像，以及进行矩阵运算等。- 在 OpenCV 中，图像被表示为
cv::Mat对象，可以通过该对象来读取、修改和操作图像数据。cv::Mat对象包含图像的像素值、通道数、尺寸等信息，并提供了一系列函数来访问和处理图像数据。
- cv::imread
cv::imread函数是 OpenCV 中用于读取图像文件的函数。其函数原型为：
cv::Mat cv::imread(const std::string& filename, int flags = cv::IMREAD_COLOR);
该函数接受两个参数：图像文件名和读取标志。其中，图像文件名是一个字符串，指定要读取的图像文件的路径和名称；读取标志是一个整数，指定要读取的图像类型。
常用的读取标志包括：
cv::IMREAD_COLOR：默认值，读取三通道彩色图像。
cv::IMREAD_GRAYSCALE：读取单通道灰度图像。
cv::IMREAD_UNCHANGED：读取原图像，包括 alpha 通道（如果有）。
cv::imread返回一个cv::Mat对象，表示读取的图像。如果无法读取图像或读取的文件格式不受支持，则该函数返回一个空的cv::Mat对象。 - cv::Mat构造函数
cv::Mat的构造函数有多个不同形式，可以接受不同的参数。常用的形参如下：cv::Mat()：默认构造函数，创建一个空的cv::Mat对象。cv::Mat(int rows, int cols, int type)：指定行数、列数和数据类型创建一个cv::Mat对象。rows：矩阵的行数。cols：矩阵的列数。type：矩阵的数据类型，例如CV_8UC1表示8位无符号单通道图像。
cv::Mat(int rows, int cols, int type, void* data, size_t step = AUTO_STEP)：指定行数、列数、数据类型以及指向数据内存的指针创建一个cv::Mat对象。rows：矩阵的行数。cols：矩阵的列数。type：矩阵的数据类型。data：指向数据内存的指针。step：可选参数，指定每行数据在内存中的步长。默认值是AUTO_STEP，表示根据矩阵的行数、数据类型自动计算步长。
cv::Mat(cv::Size size, int type)：通过指定尺寸和数据类型创建一个cv::Mat对象。size：矩阵的尺寸，使用cv::Size类型表示。type：矩阵的数据类型。
cv::Mat(cv::Size size, int type, void* data, size_t step = AUTO_STEP)：通过指定尺寸、数据类型和数据内存的指针创建一个cv::Mat对象。size：矩阵的尺寸，使用cv::Size类型表示。type：矩阵的数据类型。data：指向数据内存的指针。step：可选参数，指定每行数据在内存中的步长。默认值是AUTO_STEP，表示根据矩阵的行数、数据类型自动计算步长。
这些构造函数可以根据需要选择合适的形参来创建cv::Mat对象，并根据实际情况进行初始化和操作。
- 最难理解的地方：去畸变究竟是怎么操作的
理解这个问题的关键在于去畸变时公式操控的点也是在成像平面上的点，而不是像素平面
我们可以看double x = (u - cx) / fx, y = (v - cy) / fy这行代码，这里的 X X X实际上对应着我们公式推导过程中的 X ′ X' X′，因为使用去畸变公式的时候，我们操作的是 X X X，这也就意味着 X X X是成像平面上的点