PCL点云显示sfm数据

代码参考自：https://blog.csdn.net/YunLaowang/article/details/88388130

PCL点云

继之前的sfm，我们得到了图像的特征匹配点的三维坐标，颜色以及相机内参，如何将其显示在屏幕上呢？

需要使用PCL点云库进行显示。(安装)

PCL（Point Cloud Library）是在吸收了前人点云相关研究基础上建立起来的大型跨平台开源C++编程库，它实现了大量点云相关的通用算法和高效数据结构，涉及到点云获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等。

添加头文件：

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>

定义点云：

//-- pcl可视化 --//
PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>);

//-- 实例化PCLVisualizer对象，窗口命名为3D viewer --//
boost::shared_ptr<visualization::PCLVisualizer> viewer(new visualization::PCLVisualizer("3D viewer"));

添加三维信息和颜色信息：

for (size_t i = 0; i < pts_3d.size(); i++)
{
    //-- 定义带有三维坐标和颜色坐标的点 --//
    //-- 读取三维点的坐标 --//
    PointXYZRGB p;
    p.x = pts_3d[i].x;
    p.y = pts_3d[i].y;
    p.z = pts_3d[i].z;
    //-- 读取三维点的颜色 --//
    //-- opencv默认通道是bgr --//
    p.b = colors[i][0];
    p.g = colors[i][1];
    p.r = colors[i][2];

    cloud->points.push_back(p);
}

在相机处添加坐标系：

由于有多个相机，需要在里面显示出相机的位姿，所以需要添加多个相机坐标系

Eigen::Matrix4f transformationMatrix;
//-- 仿射变换矩阵 --//
Eigen::Affine3f postion;
for (int i = 0; i<rotations.size(); i++)
{
    transformationMatrix <<
        rotations[i].at<double>(0, 0), rotations[i].at<double>(0, 1), rotations[i].at<double>(0, 2), transformations[i].at<double>(0, 0),
    rotations[i].at<double>(1, 0), rotations[i].at<double>(1, 1), rotations[i].at<double>(1, 2), transformations[i].at<double>(1, 0),
    rotations[i].at<double>(2, 0), rotations[i].at<double>(2, 1), rotations[i].at<double>(2, 2), transformations[i].at<double>(2, 0),
    0, 0, 0, 1;
    postion.matrix() = transformationMatrix.inverse();
    //-- 0.5是摄像机的尺寸，position是摄像机的位置 --//
    viewer->addCoordinateSystem(0.5, postion);
}

添加点云：

viewer->addPointCloud<PointXYZRGB>(cloud, "sample cloud"); 
viewer->setPointCloudRenderingProperties(visualization::PCL_VISUALIZER_OPACITY, 5, "sample cloud");// 设置点云显示属性，

显示点云：

一定要写这一句，否则无法显示点云！！！

//-- 不写while循环，则窗口一闪而过 --//
while (!viewer->wasStopped()) { 
    //-- 一定要写这一句来显示点云 --//
    viewer->spinOnce(100);
}

完整代码：

// PCL可视化
PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>);
//pcl::visualization::CloudViewer viewer ("test");

boost::shared_ptr<visualization::PCLVisualizer> viewer(new visualization::PCLVisualizer("3D viewer")); // 实例化PCLVisualizer对象，窗口命名为3D viewer

for (size_t i = 0; i < pts_3d.size(); i++)
{
    PointXYZRGB p;
    p.x = pts_3d[i].x;
    p.y = pts_3d[i].y;
    p.z = pts_3d[i].z / 10.0;

    p.b = colors[i][0];
    p.g = colors[i][1];
    p.r = colors[i][2];

    cloud->points.push_back(p);
}
viewer->setBackgroundColor(0, 0, 0); // 设置背景颜色

// 在相机处添加坐标系
Eigen::Matrix4f transformationMatrix;
Eigen::Affine3f postion;
for (int i = 0; i<rotations.size(); i++)
{
    transformationMatrix <<
        rotations[i].at<double>(0, 0), rotations[i].at<double>(0, 1), rotations[i].at<double>(0, 2), transformations[i].at<double>(0, 0),
    rotations[i].at<double>(1, 0), rotations[i].at<double>(1, 1), rotations[i].at<double>(1, 2), transformations[i].at<double>(1, 0),
    rotations[i].at<double>(2, 0), rotations[i].at<double>(2, 1), rotations[i].at<double>(2, 2), transformations[i].at<double>(2, 0),
    0, 0, 0, 1;
    postion.matrix() = transformationMatrix.inverse();
    viewer->addCoordinateSystem(0.5, postion);
}


viewer->addPointCloud<PointXYZRGB>(cloud, "sample cloud"); 
viewer->setPointCloudRenderingProperties(visualization::PCL_VISUALIZER_OPACITY, 5, "sample cloud");// 设置点云显示属性，

while (!viewer->wasStopped()) { 
    viewer->spinOnce(100);
}

最终显示如图