深蓝-视觉slam-第四讲学习笔记

第四讲 相机模型 非线性优化

- 针孔相机模型与图像

- 实践：OpenCV/RGBD图像拼接

- 批量状态估计问题

- 非线性最小二乘法

- 实践：Ceres和g20(非线性优化库，图优化库）

批量的状态估计问题可以通过构建最小二乘的方式求解。
针孔相机模型：相机将三维世界中的坐标点（单位为米）映射到二维图像平面上（单位为像素）的过程。由于相机镜头上的透镜的存在，使得光线投影到成像平面的过程会产生畸变，因此，我们用针孔和畸变两个模型来描述整个投影过程。

小孔成像得到的像是倒立的，为了让模型符合实际，我们可以等价的把成像平面对称的放到相机的前面，和三维空间点一起放到摄像机坐标系的同一侧。

空间点的单位为米，不过，在相机中，我们最终获得的是一个个的像素，这还需要在成像平面上对像进采样和量化。为了描述传感器将感受到的光线转换成图像像素的过程，我们设在物理成像平面上固定着一个像素平面o-u-v.在像素平面上的到P‘的像素坐标：[u,v]T.
像素坐标系与成像平面之间，相差了一个缩放和平移，我们设像素坐标在u轴上缩放了$\alpha$倍，在v轴上缩放了$\beta$倍。同时，原点平移了[cx,cy]T，所以P‘的坐标与像素坐标[u,v]T的关系如下：

有的相机生产厂商会告诉相机的内参，而有时需要自己确定相机的内参，也就是所谓的标定.
单目棋盘格张正友标定法

相机的外参数:相机的位姿，相比于不变的内参，外参数是随着相机运动发生改变的，也是SLAM中待估计的目标，代表着机器人的轨迹。
这个模型中也可以看出，如果对相机坐标乘以任意一非零常数，归一化坐标都是一样的，这说明点的深度在投影过程中被丢失了，所以单目视觉中没法得到像素点的深度值。

在世界坐标系下有一点Pw,通过旋转和平移计算出相机坐标系下的坐标（R*Pw + t),然后除掉Z,就得到了归一化平面的坐标，再左乘相机的内参，就的到了图像中的像素值
相机的旋转和平移是slam里要估计的目标(R,t)是相机的外参，通常是我们不知道的，而内参（K）是可以通过标定确定下来的，这个是投影过程，同时也是观测过程。
观测过程：xk处观察到yj路标再加噪声的过程，xk是相机的旋转和平移，yj是路标点在世界坐标系下的坐标，zkj是观测到的像素。针孔相机描述了观测方程。

针孔相机是理想的模型，实际当中由于镜头的因素(镜头是广角，鱼眼镜头),使得图像发生变化引起了畸变

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

• 透镜自身的形状对光线传播的影响；
• 在机械组装过程中，透镜和成像平面不可能完全平行，这也会使得光线透过透镜投影到成像平面时的位置发生变化。
  径向畸变（桶形畸变 和 枕形畸变）：由透镜的形状引起的畸变，在实际的拍摄过程中会使得真实环境中的一条直线在图片中变成了曲线。越靠近图像边缘，现象越明显。透镜是中心对称的，使得这种不规则的畸变通常是径向对称的。
  桶形畸变图像放大率随着与光轴之间的距离增大而减小，而枕形畸变相反
  切向畸变:由于在相机的组装过程中不能使透镜和成像面严格平行而引起的。

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

[Xdistorted , Ydistorted]是畸变后点的归一化坐标。

3，4步骤之间补充：有畸变时，根据畸变的参数计算Pc发生畸变后的坐标。
综上所述，我们谈到了四个坐标：世界坐标，相机坐标，归一化坐标，像素坐标。它们的关系反映了整个成像过程。

单目相机的缺点：
针孔相机模型描述了单个相机的成像模型。然而仅根据一个像素，我们无法确定这个空间点的具体位置。这是因为i相机光心到归一化连线上的所有点，都可以投影到该像素上。只有当P的深度确定时，我们才能知道它的空间位置。

双目相机的 基线(记作b): 两个水平放置的相机光圈中心的距离
视差(记作d)：左右图的横坐标之差
视差与距离成反比：视差越大，距离越近。视差最小为一个像素，于是双目的深度存在一个理论上的最大值，由fb确定。基线越长，双目能测到的最大距离就越远；

双目相机的缺点：

• 虽然视差计算深度的公式简洁，但视差d本身计算比较困难，我们需要确切的知道左眼图像的某个像素在右眼中具体的位置。

• 当我们想计算每个像素的深度时，计算量和精度又是问题，而且置有在图像有纹理的地方才能计算视差。由于计算量的原因，双目深度估计仍需要使用GPU或FPGA来实时计算。

RGB-D相机能够主动测量每个像素的深度
RGB-D相机按原理分为：

• 通过红外光(Structured Light)原理测量像素距离
• 通过飞行时间(Time-of-Flight,ToF)原理测量像素距离，ToF的传感器。
  都需要向探测目标发射一束光线（通常是红外光）。在结构光原理中，返回的结构光图案，计算物体与自身之间的距离 。ToF原理中，相机和激光雷达很相似，只是激光是通过逐点扫描来测距的，而ToF相机可以获取整个图像的像素深度。
  RGB-D由摄像头，发射器和接收器组成。
  可以在同一个图像位置，读取到色彩信息和距离信息，计算像素的3D相机坐标，生成点云图（Point Cloud),所以即可以在图像层面对RGB-D数据进行处理，也可以在点云层面处理。
  RGB-D相机的缺点：
• 使用发射接收的测量，其使用范围比较受限。
• 用红外光进行测量深度值的RGB-D相机，易受日光和其他传感器发射的红外光的干扰，因此不能在室外使用。
• 在没有调制的情况下，同时使用多个RGB-D相机会相互干扰。
• 对透射材质的物体，因为接受不到反射光，无法测距，也就无法知道这些点的具体位置。
• RGB-D相机成本，功耗高

图像在数学中，可以用一个矩阵来描述；在计算机中，占据了一连串连续的磁盘或空间，可以用二维数组来表示。
计算机中的图像-——灰度图，在灰度图中，每个像素位置（x,y)对应一个灰度值I，然而计算机并不能表达实数空间 ，所以我们需要对下标和图像读数在某个范围内进行量化。
0～255的读数（一个字节)来表示图像的灰度读数。
在图像中，数组的行数对应图像的高度，列数对应图像的宽度。
访问像素 ：unsigned char pixel = image[y][x];
当我们要记录更多的信息时，一个字节恐怕不够，例如：RGB-D相机的深度图中，记录每个像素与相机之间的距离，这个距离通常是以毫米为单位的，而RGB-D的量程通常在十几米，超过了255.这时采用16位整数(unsigned short)来记录深度图的信息（0～65535），最大可达65米
彩色图像的表示需要通道的。对于每个像素，就要记录R，G，B三个值，每个值是一个通道，每个通道由8位整数来表示。彩色图共占据了24位空间。
OpenCV彩色图中，通道默认B，G，R，如果还要表达透明度，就使用R，G，B，A四个通道。

为什么需要引入最优化，无约束非线性优化等方法呐？
解决状态估计问题，为什么要进行状态估计？
由于噪声的存在，运动方程（相机成像）和观测方程（位姿由T描述，用李代数来优化）的等式不是精确的。

状态估计的方式:

• 增量/渐进(incremental)：(滤波器）观测和运动数据是随时间逐渐到来的，我们应持有一个当前时刻的状态估计，然后用新的数据来更新它。
• 批量(batch)：把0-k时刻的所有输入和观测数据放在一起，估计0-k时刻的轨迹和地图。
  显然批量是不实时的，所以我们采用折中的办法：滑动窗口法：固定一些历史轨迹，仅对当前时刻的一些轨迹进行优化。
  
  

最大似然估计的意义：“在什么样的状态下，最可能产生现在观测到的数据”


由上式的最后一项可以看出：该式等价于最小化噪声项（误差）的一个二次型，这个二次型称为马哈拉若比斯距离也叫做马氏距离
Qk,j -1叫作信息矩阵

求解上面这个方程需要我们知道关于这个目标函数的全局性质，通常这是不可能的。对于不方便求解的最小二乘问题，我们用迭代的方式

迭代的方式让求解导函数为0的问题变成了一个不断寻找下降增量$\Delta$x的问题，当函数下降直到增量非常小的时候，就认为算法收敛，目标函数达到了一个极小值。在这个寻找的过程中，如何找到每次迭代的增量，这是一个局部问题，我们只关系f在迭代值处的局部性质而不是全局性质。

最速下降法：只要我们沿着反向梯度方向前进（梯度方向是正的），在一阶（线性）下，目标函数必定会下降。

一阶和二阶中的f其实是F(x),一阶，二阶梯度法很直观，只要我们把函数在迭代点附近进行泰勒展开，并针对更新量作最小化即可。我们就是用一个一次或二次的函数来近似了原函数，然后用近似函数的最小值来猜测原函数的极小值。只要目标函数局部看起来是一次或二次函数，这种算法算是成立的。


这里的函数fx不是目标函数F(x)。

增量方程（高斯牛顿方程 或者 正规方程）：H$\Delta$x = g
高斯牛顿方用JJT作为牛顿法中的Hessian矩阵的近似从而忽略了H的计算。
高斯牛顿法的缺点：
为了求解增量方程，我们需要求解H的逆，这需要H是可逆的，但实际得到的H是JJT是半正定的，也就是说，在使用高斯牛顿法的时候，可能出现JJT为奇异矩阵或病态的情况，此时的增量的稳定性差，导致算法不收敛。直观的讲，原函数在这个点局部不像是二次函数。
即使 H非奇异非病态，如果我们求出的步长$\Delta$x太大，也会导致我们采用的局部近似不够准确，无法保证它的迭代收敛。

列文伯格-马夸尔特法修正个这个问题，它的收敛速度不高斯牛顿法满，被称为阻尼牛顿法

高斯牛顿法中采用的近似二阶泰勒展开只能在展开点有较好的近似，所以我们想到应该给$\Delta$x添加一个范围，称为信赖区域
如何确定这个范围尼？
根据近似模型与实际函数之间的差异来确定：如果差异小，说明近似好，我们应该扩大近似范围；如果差异大，就缩小近似范围；
$\rho$的分子是实际下降的值，分母是近似模型下降的值，$\rho$接近1，近似效果好，太小，说明实际减小的值远少于吉尼斯减小的值，则认为近似效果差，需要缩小近似范围；如果$\rho$较大，则说明实际的比预计的更大，我们可以放大范围

我们把增量限定在一个半径为$\mu$的球中，只有在这个球内才是有效的，带上D之后，这个球可能是个椭球。在LM中D取单位阵，相当与把增量范围约束在一个球中。

在LM中，我们用拉格朗日乘子把约束项放到目标函数中，构成拉格朗日函数：

拉格朗日函数关于$\Delta$x的导数为0，增量的线性方程为：


当参数$\lambda$比较小时，H占主要地位，说明二次近似模型在该范围内是比较好的，LM更接近于高斯牛顿法。当$\lambda$不较大时，$\lambda$ I占主要地位，LM更接近于一阶梯度法（最速下降法），说明附近二次近似不好

LM的优点：
一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定，更准确的增量。
当问题性质较好时，用GN
当问题接近病态，用LM

后期，我们会用ICP，PnP之类的算法提供优化的初始值。
实例
1，操作opencv图像

#include 
#include 
using namespace std;

#include 
#include 

int main(int argc, char **argv)
{
  cv::Mat image;
  image = cv::imread(argv[1]);
  if(image.data == nullptr) {
    cerr << "file " << argv[1] << "no exist!" << endl;
    return 0;
  }

  cout << "图像宽度：" << image.cols << ",高度：" << image.rows << ",通道数为：" << image.channels() << endl;
  cv::imshow("image", image);
  cv::waitKey(0);

  if(image.type() != CV_8UC1 && image.type() != CV_8UC3) {
    cout << "请输入一张彩色图或灰度图" << endl;
    return 0;
  }

  chrono::steady_clock::time_point t1 = 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];;
      }
    }
  }

  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 image_another = image;
  image_another(cv::Rect(0, 0, 100, 100)).setTo(0);
  cv::imshow("image", image);
  cv::waitKey(0);

  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);

  cv::destroyAllWindows();
  return 0;
}

CMakeLists.txt

project(imageBasics)
find_package(OpenCV REQUIRED)
include_directories (${OpenCV_INCLUDE_DIRS})
add_executable(imageBasics imageBasics.cpp)
# 链接OpenCV库
target_link_libraries(imageBasics ${OpenCV_LIBS})

运行结果：


2，图像去畸变

#include 
#include 

using namespace std;
string image_file = "../distorted.png";

int main(int argc, char **argv[])
{
  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);
  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++) {
      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;

}

CMakeLists.txt:

project(imageBasics)
find_package(OpenCV REQUIRED)
include_directories (${OpenCV_INCLUDE_DIRS})
add_executable(undistortImage undistortImage.cpp)
# 链接OpenCV库
target_link_libraries(undistortImage ${OpenCV_LIBS})

运行结果：

3，双目视觉，计算视差图和点云

#include 
#include 
#include 
#include 
#include  //linxu系统内置头文件，包含许多系统服务器的内置函数 如read,write, usleep等
#include 

using namespace std;
using namespace Eigen;

string left_file = "../left.png";
string right_file = "../right.png";

//点云图绘制接口，传入带一个通道的颜色信息的四维向量集
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; //相机光轴间的距离

    //读取图像（0,灰度图）
    cv::Mat left = cv::imread(left_file, 0);
    cv::Mat right = cv::imread(right_file, 0);

    //SGBM是立体匹配算法中的半全局块匹配，得到的视差图相比于BM算法来说，减少了很多不确定的匹配点，尤其是深度不连续的区域，，速度上SGBM比BM慢
    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算法匹配计算得到视差图
    sgbm->compute(left, right, disparity_sgbm);

    //将16位符号整形的视差Mat转换为32位浮点型Mat
    disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);

    vector<Vector4d, Eigen::aligned_allocator<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;
            Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0);
            
            //根据双目模型计算point的位置，计算的是左视图点的相机位置
            double x = (u - cx) / fx;  //计算的是点在归一化平面上的相机位置
            double y = (v - cy) / fy;
            double depth = fx * b / (disparity.at<float>(v, u)); //由视差，计算像素点的实际深度信息
            point[0] = x * depth; //由深度信息获取真实坐标系下的X
            point[1] = y * depth;
            point[2] = depth;

            pointcloud.push_back(point);   //获取相机坐标系下的点云位置
        }
    }
    cv::imshow("disparity" , disparity / 96.0);  //把视差限制在0-96
    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);
    }
    return ;

} 

函数解释：
参考：https://candyguo.github.io/blog-post-2/
半全局匹配算法（SGBM)是时是立体视觉中最流行的一个算法。
static Ptr cv::StereoSGBM::create (int minDisparity = 0,
int numDisparities = 16,
int blockSize = 3,
int P1 = 0,
int P2 = 0,
int disp12MaxDiff = 0,
int preFilterCap = 0,
int uniquenessRatio = 0,
int speckleWindowSize = 0,
int speckleRange = 0,
int mode = StereoSGBM::MODE_SGBM
)

参数：

• minDisparity：最小的视差值。

• numDisparity: 视差个数(64/96/128/256…)。

• blockSize：灰度相关时的窗口大小（3/5/7…) 。

• P1，P2：平滑惩罚系数，一般：P1=8通道数SADWindowSizeSADWindowSize，P2=4P1

• disp12MaxDiff ：左右视差图的最大容许差异（超过将被清零），默认为 -1，即不执行左右视差检查。

• preFilterCap：预滤波图像像素的截断值。主要是图像预处理的操作，用来排除u噪声干扰，提高边界的可区分性。

• uniquenessRatio：视差唯一性百分比， 视差窗口范围内最低代价是次低代价的(1 +
  uniquenessRatio/100)倍时，最低代价对应的视差值才是该像素点的视差，否则该像素点的视差为 0，通常为5~15.

• speckleWindowSize :平滑视差区域的最大尺寸（过滤一些斑点噪声）

• speckleRange：视差变化阈值。

• mode：模式

CMakeLists.txt:

find_package(Pangolin REQUIRED)
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
add_executable(stereoVision stereoVision.cpp)
target_link_libraries(stereoVision ${OpenCV_LIBS} ${Pangolin_LIBRARIES})

左右图像：

实现效果：

可以看到有许多空洞和视差不连续的地方，正常流程中还包括一步视差图滤波后处理。

点云图：

4，RGB-D视觉

#include 
#include 
#include 
#include 
#include 
#include 

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

void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &ponitcloud);

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));

        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;
                Eigen::Vector3d point;
                point[2] = double(d) / depthScale;    //depthScale ,我们需要把坐标都转到世界坐标系中，d是乘了缩放因子后的相机坐标系下的深度信息
                point[1] = (u - cx) * point[2]/ fx;
                point[0] = (v - cy) * point[2] / fy;
                Eigen::Vector3d pointWorld = T * point;

                Vector6d p;
                p.head<3>() = pointWorld;
            // cout << "color.step:" << color.step << endl;
                //color.step 等于像素宽度乘以3 ， 三个字节为一个像素的bgr值，第一行是从0- 3*像素宽度，第二行从第一行的基础上加就可以。
                p[5] = color.data[v * color.step + u * color.channels()];  //b
                p[4] = color.data[v * color.step + u * color.channels()+ 1]; //g
                p[3] = color.data[v * color.step + u * color.channels() + 2]; //r
                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.txt:

find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
find_package(Pangolin REQUIRED)
find_package(OpenCV 3.4.15 REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
include_directories(${Pangolin_INCLUDE_DIRS})
add_executable(joinMap joinMap.cpp)
target_link_libraries(joinMap ${OpenCV_LIBS} ${Sophus_LIBRARIES} ${Pangolin_LIBRARIES})

实现效果：