【四】相机与图像

针孔相机模型

现在来对这个简单的针孔模型进行几何建模。设$O-x-y-z$为相机坐标系，习惯上我们让$z$轴指向相机前方，$x$向右，$y$向下。$O$为摄像机的光心，也是针孔模型中的针孔。现实世界的空间点$P$，经过小孔$O$投影之后，落在物理成像平面$O^{\prime }-x^{\prime }-y^{\prime }$上、成像点为$P^{\prime }$。设$P$的坐标为$[X,Y,Z{]}^T$，$P^{\prime }$为${\left[X^{\prime },Y^{\prime },Z^{\prime }\right]}^T$，并且设物理成像平面到小孔的距离为$f$（焦距）。那么，根据三角形相似关系，有：

$$\frac{Z}{f}=-\frac{X}{X^{\prime }}=-\frac{Y}{Y^{\prime }}$$
其中负号表示成的像是倒立的。为了简化模型，我们把可以成像平面对称到相机前面，和三维空间点一起放在摄像机坐标系的同一侧（注意，后面的对称平面和归一化平面其实都是虚拟的平面），这样可以把公式中的负号去掉：
$$\frac{Z}{f}=\frac{X}{X^{\prime }}=\frac{Y}{Y^{\prime }}$$
真实的成像平面，对称的成像平面和归一化成像平面如下图所示：

整理得：
$$\begin{array}{rl}& X^{\prime }=f\frac{X}{Z}\\ & Y^{\prime }=f\frac{Y}{Z}\end{array}$$
虽然上式描述了点$P$和它的像之间的空间关系。不过，在相机中，我们最终获得的是一个个的像素，这需要在成像平面上对像进行采样和量化。为了描述传感器将感受到的光线转换成图像像素的过程，我们设在物理成像平面上固定着一个像素平面$o-u-v$。我们在像素平面得到了$P^{\prime }$的像素坐标：$[u,v{]}^T$

像素坐标系通常的定义方式是：原点$o^{\prime }$位于图像的左上角，$u$轴向右与$x$轴平行，$v$轴向下与$y$轴平行。像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。我们设像素坐标在$u$轴上缩放了$\alpha$倍，在$v$上缩放了$\beta$倍。同时，原点平移了$[c_x,c_y{]}^T$。那么，$P^{\prime }$的坐标与像素坐标$[u,v{]}^T$的关系为：
$$\{\begin{array}{l}u=\alpha X^{\prime }+c_x\\ v=\beta Y^{\prime }+c_y\end{array}$$
代入上式并把$\alpha f$合并成$f_x$，把$\beta f$合并成$f_y$,得：

$$\{\begin{array}{l}u=f_x\frac{X}{Z}+c_x\\ v=f_y\frac{Y}{Z}+c_y\end{array}$$
关注单位：$f$的单位为米，$\alpha ,\beta$的单位为像素每米，所以$f_x,f_y$的单位为像素。把该式写成矩阵形式，会更加简洁，不过左侧需要用到齐次坐标：
$$\left(\begin{array}{c}u\\ v\\ 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}{l}X\\ Y\\ Z\end{array}\right)\triangleq \frac{1}{Z}\mathbf{K}\mathbf{P}$$

按照习惯，将$Z$挪到左侧：
$$Z\left(\begin{array}{l}u\\ v\\ 1\end{array}\right)=\left(\begin{array}{ccc}{f}_x& 0& c_x\\ 0& f_y& c_y\\ 0& 0& 1\end{array}\right)\left(\begin{array}{l}X\\ Y\\ Z\end{array}\right)\triangleq \mathbf{K}\mathbf{P}$$
该式中，我们把中间的量组成的矩阵称为相机的内参数矩阵$K$。通常认为，相机的内参在出厂之后是固定的，不会在使用过程中发生变换。有的相机生产厂商会告诉你相机的内参，而有时需要自己确定相机的内参，也就是标定。

除了内参之外，相机还有相对的外参。在上式中，我们使用的是$P$在相机坐标系下的。由于相机在运动，所以$P$的相机坐标应该是它的世界坐标(记为$P_w$)，根据相机的当前位姿，变换到相机坐标系下的结果。（即P PP的坐标是相机坐标系下）。相机的位姿由它的旋转矩阵$R$和平移向量$t$来描述，那么有：
$$Z{\mathbf{P}}_{uv}=Z\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\mathbf{K}\left({\mathbf{R}\mathbf{P}}_w+\mathbf{t}\right)={\mathbf{K}\mathbf{T}\mathbf{P}}_w$$

这里第三个等号左右两边利用了齐次坐标的转换。这个齐次坐标的转换描述里$P$的世界坐标到像素坐标的投影关系。其中，相机的位姿$R,t$又称为相机的外参数。相比于不变的内参，外参会随着相机运动发生改变，用是也是SLAM中待估计的目标，代表着机器人的轨迹。

投影过程还可以从另一个角度来看，上式表明，我们可以把一个世界坐标点先转转到相机坐标系，再除掉它最后一维的数值（即该点距离相机成像平面的深度），这相当于把最后一维进行归一化处理，得到点$P$在相机归一化平面上的投影：

$$\left(R{P}_{\mathrm{w}}+t\right)=\underset{\text{相机坐标 }}{\underbrace{[X,Y,Z{]}^{\mathrm{T}}}}\to \underset{\text{归一化坐标 }}{\underbrace{[X/Z,Y/Z,1{]}^{\mathrm{T}}}}$$
归一化坐标可看成相机前方$z=1$处的平面上的一个点，这个$z=1$平面也称为归一化平面。归一化坐标再左乘内参就得到了像素坐标，所以我们可以把像素坐标$[u,v{]}^T$看成归一化平面上的点进行量化测量的结果。从这个模型中也可以看出，如果对相机坐标同时乘以任意非零常数，归一化坐标都是一样的，这说明点的深度在投影过程中被丢失了，所以单目视觉中没法得到像素点的深度值。

#include <iostream>
#include <chrono>

using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

int main(int argc, char **argv) {
  // 读取argv[1]指定的图像
  cv::Mat image;
  image = cv::imread(argv[1]); //cv::imread函数读取指定路径下的图像

  // 判断图像文件是否正确读取
  if (image.data == nullptr) { //数据不存在,可能是文件不存在
    cerr << "文件" << argv[1] << "不存在." << endl;
    return 0;
  }

  // 文件顺利读取, 首先输出一些基本信息
  cout << "图像宽为" << image.cols << ",高为" << image.rows << ",通道数为" << image.channels() << endl;
  cv::imshow("image", image);      // 用cv::imshow显示图像
  cv::waitKey(0);                  // 暂停程序,等待一个按键输入

  // 判断image的类型
  if (image.type() != CV_8UC1 && image.type() != CV_8UC3) {
    // 图像类型不符合要求
    cout << "请输入一张彩色图或灰度图." << endl;
    return 0;
  }

  // 遍历图像, 请注意以下遍历方式亦可使用于随机像素访问
  // 使用 std::chrono 来给算法计时
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for (size_t y = 0; y < image.rows; y++) 
  {
    // 用cv::Mat::ptr获得图像的行指针
    unsigned char *row_ptr = image.ptr<unsigned char>(y);  // row_ptr是第y行的头指针
    for (size_t x = 0; x < image.cols; x++) 
    {
      // 访问位于 x,y 处的像素
      unsigned char *data_ptr = &row_ptr[x * image.channels()]; // data_ptr 指向待访问的像素数据
      // 输出该像素的每个通道,如果是灰度图就只有一个通道
      for (int c = 0; c != image.channels(); c++) 
      {
        unsigned char data = data_ptr[c]; // data为I(x,y)第c个通道的值
      }
    }
  }
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast < chrono::duration < double >> (t2 - t1);
  cout << "遍历图像用时：" << time_used.count() << " 秒。" << endl;

  // 关于 cv::Mat 的拷贝
  // 直接赋值并不会拷贝数据
  cv::Mat image_another = image;
  // 修改 image_another 会导致 image 发生变化
  image_another(cv::Rect(0, 0, 100, 100)).setTo(0); // 将左上角100*100的块置零
  cv::imshow("image", image);
  cv::waitKey(0);

  // 使用clone函数来拷贝数据
  cv::Mat image_clone = image.clone();
  image_clone(cv::Rect(0, 0, 100, 100)).setTo(255);
  cv::imshow("image", image);
  cv::imshow("image_clone", image_clone);
  cv::waitKey(0);

  // 对于图像还有很多基本的操作,如剪切,旋转,缩放等,限于篇幅就不一一介绍了,请参看OpenCV官方文档查询每个函数的调用方法.
  cv::destroyAllWindows();
  return 0;
}

畸变

畸变是由透镜产生的，由透镜形状引起的畸变称为径向畸变，由透镜安装不与成像面严格平行产生的畸变为切向畸变。其中，径向畸变又分为桶形失真和枕形失真，如下图所示：

桶形畸变是由于图像放大率随着离光轴的距离增加而减小，而枕形畸变却恰好相反。在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。
利用数学方法来定义两种畸变就是，将平面上一点$p$用极坐标表示$[r,\theta {]}^T$ ，径向畸变可看成坐标点沿着长度方向发生里变化$\delta r$，也就是其距离原点的长度发生了变化。切向畸变可以看成坐标点沿着切线方向发生里变化，也就是水平夹角发生里变化$\delta \theta$。
因此，对于径向畸变，可以用和距中心距离有关的二次及高次多项式函数进行纠正：

$$\begin{array}{rl}& x_{\text{distorted}}=x\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)\\ & y_{\text{distorted}}=y\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)\end{array}$$
其中，$[x_{distorted},y_{distorted}{]}^T$是畸变后点的归一化坐标。
注意它们都是归一化平面上的点，而不是像素平面上的点。在上式描述的纠正模型中，对于畸变较小的图像中心区域，畸变主要是$k_1$起作用。而对于畸变较大的边缘区域主要是$k_2$起作用。普通摄像头用这两个系数就能很好的纠正径向畸变。对于畸变较大的摄像头，比如鱼眼镜头，可以加入$k_3$畸变项对畸变进行起作用，
另外，对于切向畸变，可以使用另外两个参数$p_1,p_2$进行纠正：
$$\begin{array}{rl}& x_{\text{distorted}}=x+2p_{1}xy+p_2\left(r^2+2x^2\right)\\ & y_{\text{distorted}}=y+p_1\left(r^2+2y^2\right)+2p_{2}xy\end{array}$$

因此，联合上两式，对于相机坐标系中的一点$P(X,Y,Z)$，我们能够通过五个畸变系数找到这个点在像素平面上的正确位置：
1.将三维空间点投影到归一化图像平面。设它的归一化坐标为$[x,y{]}^T$。
2.对归一化平面上的点计算径向畸变和切向畸变。

$$\{\begin{array}{l}{x}_{\text{distorted }}=x\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)+2p_{1}xy+p_2\left(r^2+2x^2\right)\\ y_{\text{distorted }}=y\left(1+k_1{r}^2+k_2{r}^4+k_3{r}^6\right)+p_1\left(r^2+2y^2\right)+2p_{2}xy\end{array}$$
3.将畸变后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。

$$\{\begin{array}{l}u=f_x{x}_{\text{distorted}}+c_x\\ v=f_y{y}_{\text{distorted}}+c_y\end{array}$$

在上面的纠正畸变的过程中，我们使用里五个畸变项。实际应用中，可以灵活选择纠正模型，比如只选择$k_1,p_1,p_2$这三项等。
（注：获得三项系数的方式可以利用对黑白棋盘进行标定，然后利用matlab库函数求得参数，此处不过多赘述）。
在SLAM中，常见的去畸变的做法就是先对整张图像进行畸变，得到畸变后的图像，然后讨论此图像上的点的空间位置对应到去畸变的空间位置，并将像素值复制过去。因此，当一个图像去畸变后，就可以直接用针孔模型建立投影关系，而不用考虑畸变了。
最终，总结一下单目相机的成像过程：
1.首先，世界坐标系下有一个固定的点$P$,世界坐标为$P_w$ ;
2.由于相机在运动，它的运动由$R,t$或变换矩阵$T\in SE(3)$描述。$P$的相机坐标为：${\tilde{\mathbf{P}}}_{\mathrm{c}}=R{P}_w+t$
3.此时的${\tilde{\mathbf{P}}}_{\mathrm{c}}$仍然有$X,Y,Z$三个量，把它们投影到归一化平面$Z=1$上，得到$P$的归一化相机坐标：$P_c=[X/Z,Y/Z,1{]}^T$。
4.有畸变时，根据畸变参数计算$P_c$发生畸变后的坐标。
5.$P$的归一化坐标经过内参后，对应到它的像素坐标：$P_{uv}=K{P}_c$。
注意到$Z$可能小于1，说明该点位于归一化平面后面，它可能不会在相机平面上成像，实践当中要检查一次。
综上所述，我们一共谈到了四种坐标：世界坐标、相机坐标、归一化坐标和像素坐标。需要理清他们的关系，它反应了整个成像过程。

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

using namespace std;

string image_file = "./distorted.png";   // 请确保路径正确
//string image_file = "./left.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;
}

双目相机模型

针孔相机模型描述里单目相机的成像模型，然而，该模型忽略里深度信息，也就无法确定这个空间点的具体位置的。而只有当$P$的深度确定时，我们才能确切地指导它的空间位置。
测量目标深度的方法有很多，双目相机主要依靠基线去进行测量：

根据上图，同时，根据三角形相似关系，有：

$$\frac{z-f}{z}=\frac{b-u_L+u_R}{b}$$

稍作整理，得：
$$z=\frac{fb}{d},d=u_L-u_R$$
这里$d$为左右图的横坐标之差，称为视差。根据视差，可以估计一个像素离相机的距离。视差与距离成反比：视差越大，距离越近。同时，由于视差最小为一个像素，于是双目的深度存在一个理论上的最大值，由$fb$确定。我们看到，当基线越长时，双目最大能测到的距离就会变远；反之，小型双目器件则只能测量很近的距离。
虽然由视差计算深度的公式很简洁，但视差$d$本身的计算却比较困难(需要通过特征匹配得到)。当我们想计算每个像素的深度时，其计算量与精度都将成为问题，而且只有在图像纹理变化丰富的地方才能计算视差。由于计算量的原因，双目深度估计仍需要使用GPU 或 FPGA来计算。

#include <opencv2/opencv.hpp>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>

using namespace std;
using namespace Eigen;

// 文件路径
string left_file = "./left.png";
string right_file = "./right.png";

// 在pangolin中画图，已写好，无需调整
void showPointCloud(
    const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);

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

    // 内参
    double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;
    // 基线
    double b = 0.573;

    // 读取图像
    cv::Mat left = cv::imread(left_file,0);
    cv::Mat right = cv::imread(right_file,0);
    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);

    // 生成点云
    vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;

    // 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
    for (int v = 0; v < left.rows; v++)
        for (int u = 0; u < left.cols; u++) {
            cout << disparity.at<float>(v, u) << "  \n";
            if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;

            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色

            // 根据双目模型计算 point 的位置
            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 vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud)
{

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

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

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        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();
        usleep(5000);   // sleep 5 ms
    }
    return;
}

RGB-D相机模型

RGB-D相机可以主动测量每个像素的深度，原理主要有两大类：

1.通过红外结构光来测量像素距离的。
2.通过飞行时间法原理测量像素距离的。

无论是结构光还是 ToF， RGB-D 相机都需要向探测目标发射一束光线（通常是红外光）。在结构光原理中，相机根据返回的结构光图案，计算物体离自身的距离。而在 ToF中，相机向目标发射脉冲光，然后根据发送到返回之间的光束飞行时间，确定物体离自身的距离。

ToF 原理和激光传感器十分相似，不过激光是通过逐点扫描来获取距离，而 ToF相机则可以获得整个图像的像素深度，这也正是 RGB-D 相机的特点。所以，如果你把一个 RGB-D 相机拆开，通常会发现除了普通的摄像头之外，至少会有一个发射器和一个接收器。

在测量深度之后，RGB-D相机通常按照生产时的各个相机摆放位置，自己完成深度与彩色图像素之前的配对，输出一一对应的彩色图和深度图。我们可以在同一个图像位置，读取到色彩信息和距离信息，计算像素的3D相机坐标，生成点云。对RGB-D数据，既可以在图像层面进行处理，亦可在点云层面处理。

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

#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <boost/format.hpp>  // for formating strings
#include <sophus/se3.hpp>
#include <pangolin/pangolin.h>

using namespace std;
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// 在pangolin中画图，已写好，无需调整
void showPointCloud(
    const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud);

int main(int argc, char **argv) {
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    TrajectoryType poses;         // 相机位姿

    ifstream fin("./pose.txt");
    if (!fin) {
        cerr << "请在有pose.txt的目录下运行此程序" << endl;
        return 1;
    }

    for (int i = 0; i < 5; i++) {
        boost::format fmt("./%s/%d.%s"); //图像文件格式
        colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));
        depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "pgm").str(), -1)); // 使用-1读取原始图像

        double data[7] = {0};
        for (auto &d:data)
            fin >> d;
        Sophus::SE3d pose(Eigen::Quaterniond(data[6], data[3], data[4], data[5]),
                          Eigen::Vector3d(data[0], data[1], data[2]));
        poses.push_back(pose);
    }

    // 计算点云并拼接
    // 相机内参 
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;
    vector<Vector6d, Eigen::aligned_allocator<Vector6d>> pointcloud;
    pointcloud.reserve(1000000);

    for (int i = 0; i < 5; i++) {
        cout << "转换图像中: " << i + 1 << endl;
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Sophus::SE3d T = poses[i];
        for (int v = 0; v < color.rows; v++)
            for (int u = 0; u < color.cols; u++) {
                unsigned int d = depth.ptr<unsigned short>(v)[u]; // 深度值
                if (d == 0) continue; // 为0表示没有测量到
                Eigen::Vector3d point;
                point[2] = double(d) / depthScale;
                point[0] = (u - cx) * point[2] / fx;
                point[1] = (v - cy) * point[2] / fy;
                Eigen::Vector3d pointWorld = T * point;

                Vector6d p;
                p.head<3>() = pointWorld;
                p[5] = color.data[v * color.step + u * color.channels()];   // blue
                p[4] = color.data[v * color.step + u * color.channels() + 1]; // green
                p[3] = color.data[v * color.step + u * color.channels() + 2]; // red
                pointcloud.push_back(p);
            }
    }

    cout << "点云共有" << pointcloud.size() << "个点." << endl;
    showPointCloud(pointcloud);
    return 0;
}

void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud) {

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

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

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3d(p[3] / 255.0, p[4] / 255.0, p[5] / 255.0);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}

全部的代码在这里

参考

1.《视觉SLAM十四讲》学习笔记：第5讲相机与图像
2.视觉SLAM十四讲