从零开始学习Slam(四)

从零开始学习Slam(四) - 相机成像模型

本文基本照抄计算机视觉life, 做个人笔记用

齐次坐标

首先打个基础，回忆一下齐次坐标

齐次坐标就是在原有坐标上加上一个维度.

• 如果（x, y, z）是点：

(1). 从普通坐标转换成齐次坐标

​ （x, y, z）------> (x, y, z, 1)

(2) .从齐次坐标转换成普通坐标时

​ (x, y, z, 1) ------> （x, y, z）

• 如果（x, y, z）是向量：

（1). 从普通坐标转换成齐次坐标

​ （x, y, z）------> (x, y, z, 0)

(2). 从齐次坐标转换成普通坐标时

​ (x, y, z, 0) ------> （x, y, z）

使用齐次坐标可以将加法转换为乘法，方便的表达平移，比如要将坐标点[u, v]平移t[tu, tv]
$$\left[\begin{array}{c}{u}^{\prime }\\ v^{\prime }\end{array}\right]=\left[\begin{array}{c}u+t_u\\ v+t_v\end{array}\right]=x+t$$
如果使用 齐次坐标可以表示为：

$$\left[\begin{array}{c}{u}^{\prime }\\ v^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{ccc}1& 0& t_u\\ 0& 1& t_v\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=T_x$$

如果我们想要将向量a进行一个标准的欧氏变换，一般是先用旋转矩阵R进行旋转，然后再用向量t进行平移，其结果a’ = R*a + t, 使用齐次坐标可以表达为：

$$\left[\begin{array}{c}{a}^{\prime }\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\left[\begin{array}{c}a\\ 1\end{array}\right]=T\left[\begin{array}{c}a\\ 1\end{array}\right]$$
如果进行多次欧式变换的时候就可以表达为a’ = TnTn-1…T1 a

相机成像模型

来看一张被盘到包浆的图片，相机坐标系下的点P(X, Y, Z)在相机成像平面上成的像为P’(X’, Y’, Z’) 。假设你已经知道小学二年级的小孔成像内容，根据三角形相似原理：
$$\frac{z}{f}=\frac{X}{X^{\prime }}=\frac{Y}{Y^{\prime }}\to X^{\prime }=f\frac{X}{Z},Y^{\prime }=f\frac{Y}{Z}（1）$$
其中，f是相机的焦距。

但是成像过程一般是以图像中心点为坐标系原点的，如下图所示。而我们做图像处理的时候习惯于从左上角为图像坐标系原点。如下图：

所以需要平移:

U = Uo + Cx （2）

V = Vo + Cy

这里的Uo 、Vo 、Cx、Cy是以像素为单位的数据（在图像中）， X和Y是距离单位(米或毫米)，所以还需要一个尺度因子α和β：

U = αX’+ Cx （3）

V = βY’ + Cy

Cx和Cy分别是图像宽和高的一半。

把公式（2）带入公式（1）得到：
$$\{\begin{array}{c}u=f_x\frac{X}{Z}+c_x\\ v=f_y\frac{Y}{Z}+c_y\end{array}$$
使用齐次坐标表达为：

$$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\frac{1}{Z}\left[\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]\triangleq \frac{1}{Z}KP(4)$$

这个Z是相机坐标系下P点的Z坐标，如果把这个 1/Z 和 P(X,Y,Z) 进行相乘，就得到了相机坐标系下P的归一化坐标 P = (X/Z, Y/Z, 1)

公式（4）左侧图像坐标是齐次坐标，中间红色框内的矩阵K称为内参数，最右侧蓝色框内的就是相机坐标系下的三维点P。

我们结合前面从世界坐标系到相机坐标系的变换（如公式3），就有了如下式子：

其中 fx, fy 分别是x, y方向焦距，一般都是相等的, cx,cy是光心位置，一般是长和宽的一半，他们都叫内参，此外还有畸变系数也属于内参，他们都是相机固有参数。

总结一下整个过程：

1、首先，世界坐标系下有一个三维点Pw
2、若世界坐标系到相机坐标系下的变换为旋转矩阵 R 和平移向量t 组成的变换矩阵 T，那么Pw在相机坐标系下的坐标为 Pc = R * Pw + t = T * Pw
3、此时的Pc三个分量分别是X, Y, Z，我们需要把它投影到归一化平面Z=1上，这样我们得到了相机坐标系下Pc的归一化坐标 Pc’ = (X/Z, Y/Z, 1)
4、用内参矩阵乘以归一化坐标就得到了像素坐标 Puv = K*Pc’
R * Pw + t = T * Pw
3、此时的Pc三个分量分别是X, Y, Z，我们需要把它投影到归一化平面Z=1上，这样我们得到了相机坐标系下Pc的归一化坐标 Pc’ = (X/Z, Y/Z, 1)
4、用内参矩阵乘以归一化坐标就得到了像素坐标 Puv = K * Pc’

相机畸变

当光线通过透镜时，不同位置的光线可能以不同的方式折射，导致图像边缘与中心的比例失调。这种偏差会导致几何变形（径向畸变），如桶形畸变和枕形畸变。

畸变参数

径向畸变的参数，它们是由相机标定过程确定的。径向畸变参数的值影响镜头的畸变程度，通常由标定过程自动计算得到。
下面给出畸变矫正的代码：

/*将畸变图像中一个特定位置的像素值（通过v_distorted和
u_distorted确定）复制到去畸变图像的一个指定位置（v和u）。
简单来说，它是在执行图像的逆向映射，利用畸变模型将每个去畸
变图像上的像素值从原始畸变图像中提取出来。

这种映射通常在去畸变处理过程中使用，以便将图像从畸变的状态恢复到
较为真实的状态。*/
#include <opencv2/opencv.hpp>
using namespace std;

string image_file = "./test.png";   // 图像文件的路径，确保路径正确

int main(int argc, char **argv) {
    double k1 = -0.28340811, k2 = 0.07395907;  // 畸变参数
    double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;  // 相机内参

    cv::Mat image = cv::imread(image_file, CV_8UC1);   // 读取灰度图像
    int rows = image.rows, cols = image.cols;
    cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 创建去畸变后的图像矩阵
    cv::imshow("image distorted", image);  // 显示原始畸变图像

    // 计算去畸变后图像的内容
    for (int v = 0; v < rows; v++)
        for (int u = 0; u < cols; u++) {
            double u_distorted = 0, v_distorted = 0;
            
            // 这里是需要计算去畸变后的 (u_distorted, v_distorted)
            // 开始代码
            
            double x = (u - cx) / fx;  // 计算归一化坐标
            double y = (v - cy) / fy;
            double r2 = x * x + y * y;
            double r4 = r2 * r2;
            double distortion_factor = 1 + k1 * r2 + k2 * r4;
            double x_distorted = x * distortion_factor;
            double y_distorted = y * distortion_factor;
            
            u_distorted = x_distorted * fx + cx;  // 计算去畸变后的像素坐标
            v_distorted = y_distorted * fy + 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;  // 超出范围的像素设为0
            }
        }

    cv::imshow("image undistorted", image_undistort);  // 显示去畸变图像
    cv::waitKey();  // 等待按键

    return 0;
}

• k₁和 k₂ 是径向畸变的参数，它们是由相机标定过程确定的。k₁ 和 k₂ 的值影响镜头的畸变程度，通常由标定过程自动计算得到。
• r 是像素点到图像中心的距离。为了简化计算，通常用 r²和 r⁴ 来表示畸变的影响。
• distortion_factor = 1 + k₁ * r₂ + k₂ * r₄; 是径向畸变校正中的一个因子。它表示畸变系数随着距离增加而逐渐增大的情况。通过这个因子，可以将畸变的像素值调整回相机模型的预期值。