相机与图像

在计算机中，一张照片由很多个像素组成，每个像素记录了色彩或亮度的信息，通过这些像素可以还原出原始的图像，那么这些像素是如何产生的呢？本文将针对这个问题，介绍投影关系具体如何描述，相机的内参是什么，以及双目成像与 RGB-D 相机的原理。
同时，还会介绍 OpenCV 的基本操作，包括图像的读取、显示、保存、像素遍历、复制、赋值，以及利用 OpenCV 进行相机标定、图像矫正和立体视觉的基本操作。

小孔成像模型

初中物理课本中有一个经典的蜡烛成像实验，在一个暗箱的前方放着一支点燃的蜡烛，蜡烛的光透过暗箱上的一个小孔投影在暗箱的后方平面上，并在这个平面上形成一个倒立的蜡烛图像。

在这个过程中，小孔模型能够把三维世界中的蜡烛投影到一个二维成像平面，这个将三维世界中的坐标点（单位为米）映射到二维图像平面（单位为像素）的过程就是相机成像的基本原理。
三维世界中的一个物体反射或发出的光线，穿过相机光心后，投影在相机的成像平面上。相机的感光器件接收到光线后，产生测量值，就得到了像素，形成了我们在计算机系统中见到的照片。

几何建模

下面我们将对这个简单的针孔模型进行几何建模：

$ O $ 为相机光心，也就是小孔的位置。
$ O-x-y-z $ 为相机坐标系，习惯上我们让 $ z $ 轴指向相机前方，$ x $ 向右，$ y $ 向下。
$ O’-x’-y’-z’ $ 为物理成像平面坐标系。
$ f $ 为焦距，也就是光心到成像平面的距离。
考虑空间中一个三维点 $ P $，其坐标为 $(X, Y, Z) ^ T$，经过相机光心 $ O $，在成像平面上投影出一个二维点 $P’$，其坐标为 $(X’, Y’, Z’) ^ T$。

根据相似三角形的原理，我们可以得到以下关系：
$$
\frac{X}{Z} = \frac{X’}{f} ,
\frac{Y}{Z} = \frac{Y’}{f}
$$

整理得到：
$$
X’ = \frac{X}{Z} f ,
Y’ = \frac{Y}{Z} f
$$

上面的公式描述了三维点 $ P $ 投影到成像平面上的二维坐标 $ P’ $ 的关系，这个公式中的单位是米，而在计算机中，我们使用像素来表示图像的坐标。

为了描述传感器将感受到的光线转换成图像像素，我们设在物理成像平面上固定着一个像素平面 $ O’-u-v $，像素坐标系的原点 $ O’ $ 位于图像的左上角，$ u $ 轴向右与 $ x’ $ 轴平行，$ v $ 轴向下与 $ y’ $ 轴平行。像素坐标系与成像平面之间相差了一个缩放和一个原点的平移。

我们设像素坐标在 $ u $ 轴上缩放了 $ \alpha $ 倍，在 $ v $ 轴上缩放了 $ \beta $ 倍，同时原点平移了 $ (c_x, c_y) ^ T $，那么物理成像平面上 $P’$ 的坐标 $ (X’, Y’) ^ T $ 与像素坐标 $ (u, v) ^ T $ 之间的关系可以表示为：
$$
\begin{cases}
u = \alpha X’ + c_x \\
v = \beta Y’ + c_y
\end{cases}
$$

将 $ X’ $ 和 $ Y’ $ 代入上式，得到：
$$
\begin{cases}
u = \alpha \frac{X}{Z} f + c_x \\
v = \beta \frac{Y}{Z} f + c_y
\end{cases}
$$

将 $ \alpha f $ 和 $ \beta f $ 分别记为 $ f_x $ 和 $ f_y $，得到：
$$
\begin{cases}
u = f_x \frac{X}{Z} + c_x \\
v = f_y \frac{Y}{Z} + c_y
\end{cases}
$$
其中 $ f $ 的单位是米，$ \alpha $ 和 $ \beta $ 的单位是像素/米，$ f_x $ 和 $ f_y $ 和 $ c_x $、$ c_y $ 的单位是像素。

把上面的公式写成矩阵形式：
$$
\begin{pmatrix}
u \\
v \\
1
\end{pmatrix} =
\frac{1}{Z}
\begin{pmatrix}
f_x & 0 & c_x \\
0 & f_y & c_y \\
0 & 0 & 1
\end{pmatrix}
\begin{pmatrix}
X \\
Y \\
Z
\end{pmatrix}
\triangleq \frac{1}{Z} KP.
$$

将 $ Z $ 乘到左边，得到：
$$
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}
\triangleq KP.
$$

内参矩阵

我们把上一小节中推导出的等式中的 $ K $ 称为相机的内参矩阵(Intrinsic parameters) $ K $。

相机的内参描述摄像头的成像特性，在出厂之后是固定的，不会在使用过程中发生变化。

$ f_x $ 和 $ f_y $ 是焦距(Focal Length)在 $ x $ 和 $ y $ 方向上的缩放系数。

$ c_x $ 和 $ c_y $ 是光心(Princical Point)在 $ x $ 和 $ y $ 方向上的偏移量。

此外还有畸变系数(Distortion Coefficients)，用于描述相机的畸变特性。

有的相机生产厂商会告诉你相机的内参，而有时需要你自己确定相机的内参，也就是所谓的标定。
单目棋盘格张正友标定法是一种常用的标定方法，通过拍摄棋盘格图片，可以计算出相机的内参。

外参矩阵

有内参矩阵自然就有外参矩阵(Extrinsic parameters)，外参描述了相机在世界坐标系中的位置以及其所指的方向，也就是相机坐标系与世界坐标系之间的变换关系。

考虑上诉使用的空间中的三维点 $ P $ 是在相机坐标系下的坐标，由于相机在运动，所以 $ P $ 的坐标应该是它的世界坐标系下的坐标 $ P_w $ 根据相机的当前位置和方向变换到相机坐标系下的结果。相机的位置和方向可以用旋转矩阵 $ R $ 和平移向量 $ t $ 来描述，那么 $ P $ 和 $ P_w $ 之间的关系可以表示为：
$$
Z \begin{pmatrix} u \\ v \\ 1 \end{pmatrix}
= K P = K (R P_w + t)
= K ( R \begin{pmatrix} X_w \\ Y_w \\ Z_w \end{pmatrix} + t )
= K \begin{pmatrix} R | t \end{pmatrix} \begin{pmatrix} X_w \\ Y_w \\ Z_w \\ 1 \end{pmatrix}
= K T P_w
$$

注意上述最后一个等式中，涉及齐次坐标和非齐次坐标的转换。
在齐次坐标中，三维点 $ P_w $ 的坐标是 $(X_w, Y_w, Z_w, 1) ^ T$，而在非齐次坐标中，三维点 $ P_w $ 的坐标是 $(X_w, Y_w, Z_w) ^ T$。这样做的好处是可以用一个矩阵来表示旋转和平移的变换，而不需要分别计算旋转和平移的变换。

上面的公式中，我们把 $ R | t $ 组成的矩阵称为相机的外参矩阵(Extrinsic parameters) $ T $。相比于不变的内参，外参会随着相机运动发生改变，同时也是 SLAM 中待估计的目标，代表着机器人的轨迹。

径向畸变

为了获得好的成像效果，我们在相机的前方加了透镜。透镜的加入对成像过程中光线的传播会产生新的影响：

一是透镜自身的形状对光线传播的影响。
二是在机械组装过程中，透镜和成像平面不可能完全平行，这也会使得光线穿过透镜投影到成像面时的位置发生变化。

在实际的相机成像过程中，由于透镜的形状和制造工艺等原因，会导致图像中的像素位置与实际的物体位置之间存在一定的偏差，这种由透镜形状引起的偏差称为畸变（Distortion）。畸变主要分为径向畸变(Radial Distortion)和切向畸变(Tangential Distortion)两种。

在针孔模型中，一条直线投影到像素平面上还是一条直线。可是，在实际拍摄的照片中，摄像机的透镜往往使得真实环境中的一条直线在图片中变成了曲线。越靠近图像的边缘，这种现象越明显。
由于实际加工制作的透镜往往是中心对称的，这使得不规则的畸变通常径向对称。径向畸变可以用多项式函数来描述，一般使用二阶多项式：
$$
\begin{cases}
x_{\text{corrected}} = x(1 + k_1 r^2 + k_2 r^4) \\
y_{\text{corrected}} = y(1 + k_1 r^2 + k_2 r^4)
\end{cases}
$$
其中 $ (x, y) $ 是未校正的像素坐标，$ (x_{\text{corrected}}, y_{\text{corrected}}) $ 是校正后的像素坐标，$ r = \sqrt{x^2 + y^2} $ 是像素到光心的距禂，$ k_1 $ 和 $ k_2 $ 是径向畸变系数，有畸变时，要根据根据畸变参数计算发生畸变后的坐标，对投影矩阵做一定的修正。

双目相机

针孔相机模型描述了单个相机的成像模型。然而，仅根据一个像素，我们无法确定这个空间点的具体位置。只有当深度信息被引入时，我们才能确定空间点的具体位置。

双目相机有两个摄像头，两个摄像头的优点在于可以同时拍摄同一个场景的两个视角，这样就可以通过视差来计算空间点的深度信息。

在左右双目相机中，我们可以把两个相机都看作针孔相机。它们是水平放置的，意味着两个相机的光圈中心都位于 $ x $ 轴上。两者之间的距离称为双目相机的基线（Baseline，记作 $ b $），基线是双目相机的重要参数。

考虑一个空间点 $ P $，在左右两个相机中的投影分别为 $ P_L $ 和 $ P_R $，由于左右相机只在水平方向 $ x $ 轴上有一定的位移，所以 $ P_L $ 和 $ P_R $ 的 $ y $ 和 $ z $ 坐标是相同的，只有 $ x $ 坐标不同，我们记它的左侧坐标为 $ u_L $、右侧坐标为 $ u_R $，根据 $ \triangle P P_L P_R $ 和 $ \triangle P O_L O_R $ 的相似性，我们可以得到：
$$
\frac{z - f}{z} = \frac{b - u_L + u_R}{b}
$$
整理得：
$$
z = \frac{fb}{d}, \quad d = u_L - u_R
$$
其中 $ d $ 是视差，定义为左右图的横坐标之差，根据视差可以计算出空间点的深度信息。视差与距离 $ z $ 成反比，视差越大，物体离相机越近。
由于视差最小为一个像素，于是双目的深度存在一个理论的最大值，由 $ fb $ 决定。基线 $ b $ 越长，双目所能测的最大距离就越远。可以用生活中人眼看非常远处的物理（如山脉）来类比，通常不能准确测量远处物体的深度。

虽然由视差计算深度的公式很简洁，但视差 $ d $ 本身的计算却比较困难。我们需要确切地知道左眼图像某个像素出现在右眼图像的哪一个位置（即对应关系），这件事亦属于“人类觉得容易而计算机觉得困难”的任务。当我们想计算每个像素的深度时，其计算量与精度都将成为问题，而且只有在图像纹理变化丰富的地方才能计算视差。

RGB-D相机

RGB-D 相机是一种同时具有彩色图像和深度图像的相机，它可以在一个设备中同时获取彩色图像和深度图像，是一种非常方便的设备。RGB-D 相机的工作原理是通过结构光、飞行时间、双目视差等技术主动获取深度信息。

RGB-D 相机都需要向探测目标发射一束光线（通常是红外光），然后通过接收器接收反射回来的光线，通过测量光线的时间差、光线的强度和图案等信息来计算目标的深度信息。

RGB-D 相机能够实时地测量每个像素点的距离。但是，由于这种发射 − 接收的测量方式，其使用范围比较受限。用红外光进行深度值测量的 RGB-D 相机，容易受到日光或其他传感器发射的红外光干扰，因此不能在室外使用。在没有调制的情况下，同时使用多个 RGB-D 相机时也会相互干扰。对于透射材质的物体，因为接收不到反射光，所以无法测量这些点的位置。此外，RGB-D 相机在成本、功耗方面，都有一些劣势。

图像

相机加上镜头，把三维世界中的信息转换成了一张由像素组成的照片，随后存储在计算机中，作为后续处理的数据来源。

在数字中，图像可以用一个矩阵来表示。
在计算机中，图像占据一段连续的磁盘或内存空间，可以用二维数组来表示。
在 OpenCV 中，图像是一个 Mat 类型的对象，可以用 cv::Mat 类来表示。

这样一来，程序就不必区分它们处理的是一个数值矩阵还是有实际意义的图像了。

灰度图像

灰度图像只包含一个通道，表示图像的亮度或灰度级别，而不包含颜色信息。在常见的灰度图中，用 0 ~ 255 的整数(即一个unsigned char， 1个字节)来表示像素的灰度值，0 表示黑色，255 表示白色。一张宽度为 640 像素，高度为 480 像素分辨率的灰度图像，就可以表示为：

1	unsigned char image[480][640];

深度图像

深度图像是一种特殊的图像，它的每个像素值表示的是该像素点到相机的距离。

在 RGB-D 相机的深度图中，记录了各个像素与相机之间的距离。这个距离通常是以毫米为单位，而 RGB-D 相机的量程通常在十几米左右，超过了灰度图像的 0 ~ 255 的范围。这时，我们可以用一个 16 位整数来表示深度图像的像素值，即一个 unsigned short 类型的整数。一张宽度为 640 像素，高度为 480 像素分辨率的深度图像，就可以表示为：

1	unsigned short depth_image[480][640];

彩色图像

彩色图像的表示则需要通道（channel）的概念。在计算机中，我们用红色、绿色和蓝色这三种颜色的组合来表达任意一种色彩。于是对于每一个像素，就要记录其 $ R、G、B $ 三个数值，每一个数值就称为一个通道。例如，最常见的彩色图像有三个通道，每个通道都由 8 位整数表示。在这种规定下，一个像素占据 24 位空间。

通道的数量、顺序都是可以自由定义的。在 OpenCV 的彩色图像中，通道的默认顺序是 $ B、G、R $。也就是说，当我们得到一个 24 位的像素时，前 8 位表示蓝色数值，中间 8 位为绿色，最后 8 位为红色。同理，亦可使用 $ R、G、B $ 的顺序表示一个彩色图。如果还想表达图像的透明度，就使用 $ R、G、B、A $ 四个通道。

一张宽度为 640 像素，高度为 480 像素分辨率的彩色图像，就可以表示为：

1	unsigned char image[480][640][3];

除了 RGB 通道外，还有其他的颜色空间，如 HSV、YUV、Lab 等。

HSV（Hue, Saturation, Value）是一种常用的颜色空间，其中 H 表示色调，S 表示饱和度，V 表示亮度。
YUV 是一种将亮度和色度分离的颜色空间，其中 Y 表示亮度，U 和 V 表示色度。
Lab 是一种颜色空间，其中 L 表示亮度，a 和 b 表示颜色。

在 OpenCV 中，可以通过 cv::cvtColor 函数来进行颜色空间的转换。

OpenCV实践

OpenCV 提供了大量的开源图像算法，是计算机视觉中使用极广的图像处理算法库，下面通过一个演示程序来理解，在 OpenCV 中图像是如何存取，而我们又是如何访问其中的像素的。

安装

安装依赖

1	sudo apt−get install build−essential libgtk2.0−dev libvtk5−dev libjpeg−dev libtiff4−dev libjasper−dev libopenexr−dev libtbb−dev

下载 OpenCV 源码

1
2
3

wget https://opencv.org/releases/opencv-xxx.zip
unzip opencv-xxx.zip
cd opencv-xxx

编译安装

mkdir build
cd build
cmake ..
make -j4
sudo make install

也可以使用 Conan 安装 OpenCV，具体可以参考 Conan 安装 OpenCV

图像基本操作

读取、显示、像素遍历、复制、赋值、旋转、裁剪、缩放等基本操作。

展开查看代码

#include <chrono>
#include <iostream>

#include "opencv2/core/mat.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/opencv.hpp"

int main(int argc, char** argv) {
    if (argc != 2) {
        std::cerr << "usage: imageBasics <path_to_image>" << std::endl;
        return -1;
    }
    cv::Mat image = cv::imread(argv[1]);
    if (image.data == nullptr) {
        std::cerr << "file " << argv[1] << " does not exist." << std::endl;
        return 0;
    }
    if (image.type() != CV_8UC1 && image.type() != CV_8UC3) {
        std::cout << "please input a color or gray image." << std::endl;
        return 0;
    }

    // Output image information
    std::cout << "width: " << image.cols << ", height: " << image.rows << ", channels: " << image.channels() << std::endl;
    cv::imshow("Original Image", image);
    cv::waitKey(0);

    // Crop image
    cv::Rect roi(50, 50, 200, 200);
    cv::Mat cropped_image = image(roi);
    cv::imshow("Cropped Image", cropped_image);
    cv::waitKey(0);

    // Rotate image
    cv::Point2f center(image.cols / 2.0, image.rows / 2.0);
    double angle = 45.0;
    double scale = 1.0;
    cv::Mat rotation_matrix = cv::getRotationMatrix2D(center, angle, scale);
    cv::Mat rotated_image;
    cv::warpAffine(image, rotated_image, rotation_matrix, image.size());
    cv::imshow("Rotated Image", rotated_image);
    cv::waitKey(0);

    // Resize image
    cv::Mat resized_image;
    cv::resize(image, resized_image, cv::Size(), 0.5, 0.5);
    cv::imshow("Resized Image", resized_image);
    cv::waitKey(0);

    cv::destroyAllWindows();

    // Access pixel
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    for (size_t y = 0; y < image.rows; y++) {
        unsigned char* row_ptr = image.ptr<unsigned char>(y);
        for (size_t x = 0; x < image.cols; x++) {
            unsigned char* data_ptr = &row_ptr[x * image.channels()];
            for (int c = 0; c != image.channels(); c++) {
                unsigned char data = data_ptr[c];
            }
        }
    }
    std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used = std::chrono::duration_cast<std::chrono::duration<double>>(t2 - t1);
    std::cout << "time used on accessing pixel: " << time_used.count() << " seconds." << std::endl;

    // Assignment does not copy data
    cv::Mat image_another = image;
    image_another(cv::Rect(0, 0, 100, 100)).setTo(0); // Set the top-left corner to black
    cv::imshow("image", image);
    cv::waitKey(0);

    // Clone does copy data
    cv::Mat image_clone = image.clone();
    image_clone(cv::Rect(0, 0, 100, 100)).setTo(255); // Set the top-left corner to white
    cv::imshow("image", image);
    cv::imshow("image_clone", image_clone);
    cv::waitKey(0);

    cv::destroyAllWindows();

    return 0;
}

图像去畸变

从公式出发计算畸变前后的图像坐标，然后根据畸变参数来计算去畸变后的图像坐标。

展开查看代码

#include <iostream>

#include "opencv2/opencv.hpp"

int main(int argc, char** argv) {
    if (argc != 2) {
        std::cerr << "usage: undistortImage <path_to_image>" << std::endl;
        return -1;
    }

    // distortion parameters
    double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
    // intrinsic parameters
    double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;

    cv::Mat image = cv::imread(argv[1], cv::IMREAD_GRAYSCALE);
    int rows = image.rows, cols = image.cols;
    cv::Mat undistorted_image = cv::Mat::zeros(rows, cols, CV_8UC1);

    for (int v = 0; v < rows; v++) {
        for (int u = 0; u < cols; u++) {
            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;
            int u_distorted = fx * x_distorted + cx;
            int v_distorted = fy * y_distorted + cy;
            if (u_distorted >= 0 && u_distorted < cols && v_distorted >= 0 && v_distorted < rows) {
                undistorted_image.at<uchar>(v, u) = image.at<uchar>(v_distorted, u_distorted);
            } else {
                undistorted_image.at<uchar>(v, u) = 0;
            }
        }
    }

    cv::imshow("Original Image", image);
    cv::imshow("Undistorted Image", undistorted_image);
    cv::waitKey(0);

    return 0;
}

图像标定

通过拍摄棋盘格图片，计算相机的内参。

展开查看代码

#include <iostream>
#include <vector>

#include "opencv2/opencv.hpp"

int main(int argc, char** argv) {
    if (argc != 2) {
        std::cerr << "usage: calibration <dir_to_images>" << std::endl;
        return -1;
    }
    std::string base_dir = argv[1];

    int board_width = 9;
    int board_height = 6;
    int num_squares = board_width * board_height;
    float square_size = 1.0f;

    std::vector<cv::Point3f> object_points;
    for (int i = 0; i < board_height; i++) {
        for (int j = 0; j < board_width; j++) {
            object_points.push_back(cv::Point3f(j * square_size, i * square_size, 0));
        }
    }

    std::vector<std::vector<cv::Point3f>> object_points_list;
    std::vector<std::vector<cv::Point2f>> image_points_list;

    for (int i = 1; i <= 10; i++) {
        std::string file_name = "calibration" + std::to_string(i) + ".png";
        std::string file_path = base_dir + file_name;
        cv::Mat image = cv::imread(file_path, cv::IMREAD_GRAYSCALE);
        if (image.empty()) {
            std::cerr << "Cannot open image: " << file_path << std::endl;
            continue;
        }

        std::vector<cv::Point2f> image_points;
        bool found = cv::findChessboardCorners(image, cv::Size(board_width, board_height), image_points);
        if (found) {
            cv::cornerSubPix(image, image_points, cv::Size(11, 11), cv::Size(-1, -1),
                             cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));
            image_points_list.push_back(image_points);
            object_points_list.push_back(object_points);
        }

        cv::drawChessboardCorners(image, cv::Size(board_width, board_height), image_points, found);
        cv::imshow("Calibration", image);
        cv::waitKey(500);
    }

    cv::Mat camera_matrix, dist_coeffs;
    std::vector<cv::Mat> rvecs, tvecs;
    cv::calibrateCamera(object_points_list, image_points_list, cv::Size(board_width, board_height),
                        camera_matrix, dist_coeffs, rvecs, tvecs);

    std::cout << "Camera Matrix: " << camera_matrix << std::endl;
    std::cout << "Distortion Coefficients: " << dist_coeffs << std::endl;

    return 0;
}

双目视觉

从双目的左右图像出发，计算图像对应的视差图，然后再计算各像素在相机坐标系下的坐标，它们将构成点云。

展开查看代码

#include <iostream>
#include <string>
#include <vector>

#include "Eigen/Core"
#include "opencv2/opencv.hpp"
#include "pangolin/pangolin.h"

void showPointCloud(const std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>>& pointcloud);

int main(int argc, char** argv) {
    if (argc != 3) {
        std::cerr << "Usage: stereoVision left_image right_image" << std::endl;
        return -1;
    }
    std::string left_file = argv[1];
    std::string right_file = argv[2];

    // Intrinsic parameters and baseline
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    double b = 0.573;

    cv::Mat left = cv::imread(left_file, cv::IMREAD_GRAYSCALE);
    cv::Mat right = cv::imread(right_file, cv::IMREAD_GRAYSCALE);
    if (left.empty() || right.empty()) {
        std::cerr << "Error: Could not load images!" << std::endl;
        return -1;
    }

    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
            0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);
    cv::Mat disparity_sgbm, disparity;
    sgbm->compute(left, right, disparity_sgbm);
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);

    std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>> pointcloud;

    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) {
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;

            Eigen::Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0);

            double x = (u - cx) / fx;
            double y = (v - cy) / fy;
            double depth = fx * b / (disparity.at<float>(v, u));
            point[0] = x * depth;
            point[1] = y * depth;
            point[2] = depth;

            pointcloud.push_back(point);
        }

    cv::imshow("disparity", disparity / 96.0);
    cv::waitKey(0);

    showPointCloud(pointcloud);

    return 0;
}

void showPointCloud(const std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>>& pointcloud) {
    if (pointcloud.empty()) {
        std::cerr << "Point cloud is empty!" << std::endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 420, 420, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(-1, -1, -1, 0, 0, 0, pangolin::AxisY));

    pangolin::View& d_cam = pangolin::CreateDisplay()
                                    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
                                    .SetHandler(new pangolin::Handler3D(s_cam));

    while (!pangolin::ShouldQuit()) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto& p: pointcloud) {
            glColor3f(p[3], p[3], p[3]);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
    }
}

RGB-D视觉

通过 RGB-D 相机获取深度图像，然后根据深度信息计算点云。

根据内参计算一对 RGB-D 图像对应的点云
根据各张图的相机位姿（也就是外参），把点云加起来，组成地图。

展开查看代码

#include <iostream>
#include <string>
#include <vector>

#include "Eigen/Core"
#include "opencv2/opencv.hpp"
#include "pangolin/pangolin.h"

void showPointCloud(const std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>>& pointcloud);

int main(int argc, char** argv) {
    if (argc != 3) {
        std::cerr << "Usage: stereoVision left_image right_image" << std::endl;
        return -1;
    }
    std::string left_file = argv[1];
    std::string right_file = argv[2];

    // Intrinsic parameters and baseline
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    double b = 0.573;

    cv::Mat left = cv::imread(left_file, cv::IMREAD_GRAYSCALE);
    cv::Mat right = cv::imread(right_file, cv::IMREAD_GRAYSCALE);
    if (left.empty() || right.empty()) {
        std::cerr << "Error: Could not load images!" << std::endl;
        return -1;
    }

    cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(
            0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);
    cv::Mat disparity_sgbm, disparity;
    sgbm->compute(left, right, disparity_sgbm);
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);

    std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>> pointcloud;

    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) {
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;

            Eigen::Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0);

            double x = (u - cx) / fx;
            double y = (v - cy) / fy;
            double depth = fx * b / (disparity.at<float>(v, u));
            point[0] = x * depth;
            point[1] = y * depth;
            point[2] = depth;

            pointcloud.push_back(point);
        }

    cv::imshow("disparity", disparity / 96.0);
    cv::waitKey(0);

    showPointCloud(pointcloud);

    return 0;
}

void showPointCloud(const std::vector<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d>>& pointcloud) {
    if (pointcloud.empty()) {
        std::cerr << "Point cloud is empty!" << std::endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 420, 420, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(-1, -1, -1, 0, 0, 0, pangolin::AxisY));

    pangolin::View& d_cam = pangolin::CreateDisplay()
                                    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
                                    .SetHandler(new pangolin::Handler3D(s_cam));

    while (!pangolin::ShouldQuit()) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        d_cam.Activate(s_cam);
        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto& p: pointcloud) {
            glColor3f(p[3], p[3], p[3]);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
    }
}

sitJac's Blog

相机与图像

小孔成像模型

几何建模

内参矩阵

外参矩阵

径向畸变

双目相机

RGB-D相机

图像

灰度图像

深度图像

彩色图像

OpenCV实践

安装

图像基本操作

图像去畸变

图像标定

双目视觉

RGB-D视觉

拓展阅读