视觉slam十四讲ch5相机与图像（包含实践部分）-CSDN博客

1.相机模型

相机将三维世界中的坐标点(单位为米)映射到二维图像平面(单位为像素)的过程能够用一个几何模型进行描述。

这个模型有很多种，有最常见的针孔模型和由于相机镜头上的透镜的存在，使得光线投影到成像平面的过程中会产生畸变的畸变模型。这两个模型能够把外部的三维点投影到相机内部成像平面，构成相机的内参数(Intrinsics)

1.1针孔相机模型

对其建模，

设O-x-y-z相机坐标系，让z轴指向相机前方，x轴向右，y轴向下(此图我们应该站在左侧看右侧)。O为摄像机的光心，也是针孔模型中的针孔。现实世界的空间点P，经过小孔O投影之后，落在物理成像平面O'-x'-y'上，成像点为P'。设P的坐标为[x,y,z]^{T}，P'为[x',y']^{T}，并且设物理成像平面到小孔的距离为f(焦距）。那么，根据三角形相似关系，有：

负号表示倒立，去除负号为：

整理得：

这就是针孔模型。

像素坐标

在相机中，我们最终获得的是一个个的像素，这还需要在成像平面上对像进行采样和量化。为了描述传感器将感受到的光线转换成图像像素的过程，我们设在物理成像平面上固定着一个像素平面 $o-u-v$ 。我们在像素平面得到了 $P'$ 的像素坐标： $[u,v]^{T}$

像素坐标系通常的定义方式为原点 $o'$ 位于图像的左上角， $u$ 轴向右与 $x$ 轴平行， $v$ 轴向下与 $y$ 轴平行。像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。我们设像素坐标在 $u$ 轴上缩放了 $\alpha$ 倍，在 $v$ 轴上缩放了 $\beta$ 倍。同时，原点平移了 $[C_{x},C_{y}]^{T}$ 。那么， $P'$ 的坐标与像素坐标 $[u,v]^{T}$ 的关系为：

代入针孔模型式中得：

写为矩阵形式：

整理得：

相机内外参

该式中，我们把中间的量组成的矩阵称为相机的内参数(Camera Intrinsics)矩阵K。通常认为，相机的内参在出厂之后是固定的，不会在使用过程中发生变化。有的相机生产厂商会告诉你相机的内参，而有时需要你自己确定相机的内参，也就是所谓的标定。

内参有了，外参是什么呢，P为相机坐标，是由它的世界坐标Pw根据相机位姿R，t变换过来，有：

R，t则为相机的外参，随着相机运动不断变化，是slam中待估计的目标。

归一化坐标

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

归一化坐标可看成相机前方z=1处的平面上的一个点，这个z=1平面也称为归一化平面。归一化坐标再左乘内参就得到了像素坐标，所以我们可以把像素坐标[u,v]^{T}看成对归一化平面上的点进行量化测量的结果。

从这个模型中也可以看出，如果对相机坐标同时乘以任意非零常数，归一化坐标都是一样的，这说明点的深度在投影过程中被丢失了，所以单目视觉中没法得到像素点的深度值。

1.2畸变模型

产生畸变的原理：为了获得好的成像效果，我们在相机的前方加了透镜。透镜的加入会对成像过程中光线的传播产生新的影响：一是透镜自身的形状对光线传播的影响。二是在机械组装过程中，透镜和成像平面不可能完全平行，这也会使光线穿过透镜投影到成像面时的位置发生变化。

由透镜形状引起的畸变（Distortion，也叫失真）称为径向畸变。（直线在图片中变成曲线，越在边缘越明显，所以说拍照要站c位哈）分为两大类：桶形畸变和枕形畸变。

桶形畸变图像放大率随着与光轴之间的距离增加而减小而枕形畸变则恰好相反。在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。
除了透镜的形状会引入径向畸变，由于在相机的组装过程中不能使透镜和成像面严格平行,所以也会引人切向畸变。

径向畸变可以看成坐标点沿着长度方向发生了变化，也就是其距离原点的长度发生了变化。切向畸变可以看成坐标点沿着切线方向发生了变化，也就是水平夹角发生了变化。

通常假设这些畸变呈多项式关系，即：

这是畸变后点的归一化坐标，对于切向畸变，有另两个参数p1，p2：

设归一化坐标为（x，y），则对归一化平面上的点进行径向畸变和切向畸变：

将畸变后的点通过内参投到像素平面，得到该点在图像上的正确位置：

总结，像素坐标是先归一化再进行去畸变再计算成像素坐标的。

在slam中针孔模型和径向及切向模型已经足够，且在后文中，直接假设图像已经进行了去畸变处理。

讨论单目成像过程

1.4双目相机模型

针孔相机模型描述了单个相机的成像模型。然而，仅根据一个像素，我们无法确定这个空间点的具体位置（仅凭照过的照片，无法还原当时的3D情况）。这是因为，从相机光心到归一化平面连线上的所有点，都可以投影至该像素上。只有当Р的深度确定时（比如通过双目或RGB-D相机)，我们才能确切地知道它的空间位置。

测量像素距离(或深度)的方式有很多种，比如人眼就可以根据左右眼看到的景物差异(或称视差)判断物体与我们的距离。双目相机的原理亦是如此：通过同步采集左右相机的图像，计算图像间视差，以便估计每一个像素的深度。

下面简单介绍双目相机的成像原理：

在左右双目相机中，我们可以把两个相机都看作针孔相机。它们是水平放置的,意味着两个相机的光圈中心都位于x轴上。两者之间的距离称为双目相机的基线(记作b)，是双目相机的重要参数。
现在，考虑一个空间点P，它在左眼相机和右眼相机各成一像，记作Pl,Pr。由于相机基线的存在，这两个成像位置是不同的。理想情况下，由于左右相机只在x轴上有位移，所以P的像也只在x轴(对应图像的u轴)上有差异。记它的左侧坐标为ul，右侧坐标为ur，几何关系如图右侧所示。根据三角形的相似关系，有：

整理得：

其中d定义为左右图的横坐标之差，称为视差。根据视差，我们可以估计一个像素与相机之间的距离。视差与距离成反比：视差越大，距离越近。同时，由于视差最小为一个像素，于是双目的深度存在一个理论上的最大值，由fb确定。我们看到，基线越长，双目能测到的最大距离就越远；反之，小型双目器件则只能测量很近的距离。相似地，我们人眼在看非常远的物体时（如很远的飞机)，通常不能准确判断它的距离。
虽然由视差计算深度的公式很简洁，但视差d本身的计算却比较困难。我们需要确切地知道左眼图像的某个像素出现在右眼图像的哪一个位置(即对应关系)，这件事也属于“人类觉得容易而计算机觉得困难”的任务。当我们想计算每个像素的深度时，其计算量与精度都将成为问题，而且只有在图像纹理变化丰富的地方才能计算视差。由于计算量的原因，双目深度估计仍需要使用GPU或FPGA来实时计算。

1.5RGB—D相机模型（海豚、蝙蝠原理）

相比于双目相机通过视差计算深度的方式，RGB-D相机的做法更“主动”，它能够主动测量每个像素的深度。目前的RGB-D相机按原理可分为两大类：
        ①通过红外结构光(Structured Light)原理测量像素距离。例子有Kinect 1代、Project Tango 1代、Intel ReaSense等。
        ②通过飞行时间(Time-of-Flight，ToF)原理测量像素距离。例子有Kinect 2代和一些现有的ToF传感器等。

无论是哪种类型，RGB-D相机都需要向探测目标发射一束光线(通常是红外光)。在红外结构光原理中，相机根据返回的结构光图案，计算物体与自身之间的距离。而在ToF原理中，相机向目标发射脉冲光，然后根据发送到返回之间的光束飞行时间，确定物体与自身的距离。ToF的原理和激光传感器十分相似，只不过激光是通过逐点扫描获取距离的，而ToF相机则可以获得整个图像的像素深度，这也正是RGB-D相机的特点。所以，如果把一个RGB-D相机拆开，通常会发现除了普通的摄像头，至少会有一个发射器和一个接收器。
        在测量深度之后，RGB-D相机通常按照生产时的各相机摆放位置，自己完成深度与彩色图像素之间的配对，输出一一对应的彩色图和深度图。我们可以在同一个图像位置，读取到色彩信息和距离信息，计算像素的3D相机坐标，生成点云(Point Cloud)。既可以在图像层面对RGB-D数据进行处理，也可在点云层面处理。本讲的第二个实验将演示RGB-D相机的点云构建过程。
        RGB-D相机能够实时地测量每个像素点的距离。但是，由于使用这种发射–接收的测量方式，其使用范围比较受限。用红外光进行深度值测量的RGB-D相机，容易受到日光或其他传感器发射的红外光干扰，因此不能在室外使用。在没有调制的情况下，同时使用多个RGB-D相机此时也会相互干扰。对于透射材质的物体，因为接收不到反射光，所以无法测量这些点的位置。此外，RGB-D相机在成本、功耗方面都有一些劣势。

2.图像

用灰度图举例，在一张灰度图中，每个像素位置 $(x,y)$ 对应一个灰度值 $I$ ，所以，一张宽度为 $w$ 、高度为 $h$ 的图像，数学上可以记为一个函数：

在常见的灰度图中，用0~255的整数(即一个unsigned char,1个字节)来表达图像的灰度读数。

图像坐标示意图可以表示为：

这块之后自己了解一下。

3.实践：计算机中的图像

3.1Opencv的基本使用方法：

安装Opencv：

安装依赖项：
sudo apt-get install build-essential
sudo apt-get install cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev
sudo apt-get install python-dev python-numpy libtbb2 libtbb-dev libjpeg-dev libpng-dev libtiff-dev libjasper-dev libdc1394-22-dev
我下的是3.4.15版本，找到github网页https://github.com/opencv/opencv/tree/3.4.15，下载压缩包并解压到指定文件夹中

在文件夹中编译安装：
mkdir build
cd build
cmake -D CMAKE_BUILD_TYPE=Release -D CMAKE_INSTALL_PREFIX=/usr/local ..
make
sudo make install
安装成功

配置Opencv的编译环境:
sudo gedit /etc/ld.so.conf.d/opencv.conf 
打开空白文件，在空白文件中添加：
/usr/local/lib 
保存并回到终端，执行命令使之生效：
sudo ldconfig
配置bash
sudo gedit /etc/bash.bashrc
在末尾添加：
PKG_CONFIG_PATH=$PKG_CONFIG_PATH:/usr/local/lib/pkgconfig  
export PKG_CONFIG_PATH  
保存并执行命令使之生效：
source /etc/bash.bashrc
可以验证一下:
cd opencv/samples/cpp/example_cmake
编译并执行，笔者的Opencv版本开启电脑摄像头并显示Hello Opencv，安装成功。

操作Opencv图像：

修改CMakeLists为：

cmake_minimum_required( VERSION 2.8 )
project(imageBasics)
 
set( CMAKE_CXX_FLAGS "-std=c++11 -O3")
find_package(OpenCV)
include_directories(${OpenCV})
 
add_executable(imageBasics imageBasics.cpp)

target_link_libraries(imageBasics ${OpenCV_LIBS})
 
add_executable(undistortImage undistortImage.cpp)

target_link_libraries(undistortImage ${OpenCV_LIBS})

code：

#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;
}

执行命令为： build/imageBasics ubuntu.png

结果为：

图像去畸变：

修改书上代码，将图片存储位置改为绝对路径：/home/shikai/github/slambook2/ch5/imageBasics/distorted.png

code：

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

using namespace std;

string image_file = "/home/shikai/github/slambook2/ch5/imageBasics/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;
}

执行结果为：

4.实践：3D视觉

4.1双目视觉：

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

依旧将图片保存位置设为绝对路径：

code：

#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 = "/home/shikai/github/slambook2/ch5/stereo/left.png";
string right_file = "/home/shikai/github/slambook2/ch5/stereo/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++) {
            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;
}

CMakeLists:

cmake_minimum_required( VERSION 2.8 )
project(stereoVision)
set( CMAKE_CXX_FLAGS "-std=c++11 -O3")

include_directories("/usr/include/eigen3")
find_package(Pangolin REQUIRED)
include_directories( ${Pangolin_INCLUDE_DIRS} )

find_package(OpenCV)
include_directories(${OpenCV})

add_executable(stereoVision stereoVision.cpp)
target_link_libraries(stereoVision ${OpenCV_LIBS} ${Pangolin_LIBRARIES})

执行结果为：

右上为SGBM的视差图，右下为点云图。

此例程调用SGBM算法计算左右图像的视差，通过双目相机的几何模型把它变换到相机的3D空间中。

4.2RGB-D视觉

RGB-D相机的方便之处在于能通过物理方法获得像素深度信息。若已知相机的内外参，我们可以计算任何一个像素在世界坐标系下的位置，从而建立一张点云地图

准备了五对图像，有五张RGB图五张深度图，又同时给出五张图像的相机内外参位姿，下面这段程序完成两件事：

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

code：

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



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("/home/shikai/github/slambook2/ch5/rgbd/pose.txt");
    if (!fin) {
        cerr << "请在有pose.txt的目录下运行此程序" << endl;
        return 1;
    }

    for (int i = 0; i < 5; i++) {
        boost::format fmt("/home/shikai/github/slambook2/ch5/rgbd/%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;
}

CMakeLists:

cmake_minimum_required( VERSION 2.8 )
project(joinMap)
set( CMAKE_CXX_FLAGS "-std=c++11 -O3")

include_directories("/usr/include/eigen3")
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

find_package(Pangolin REQUIRED)
include_directories( ${Pangolin_INCLUDE_DIRS} )

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(joinMap joinMap.cpp)
target_link_libraries(joinMap ${OpenCV_LIBS} ${Pangolin_LIBRARIES})

执行完结果为：