读取一系列深度图和彩色图以及它们的位姿

创建三个容器

vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    vector<Eigen::Isometry3d> poses;         // 相机位姿

读pose.txt

ifstream fin("/home/linhd/slambook2-master/ch12/dense_RGBD/data/pose.txt");
    if (!fin) {
        cerr << "cannot find pose file" << endl;
        return 1;
    }

boost::format 格式化字符串包含占位符 %s、%d 和 %s，分别表示文件夹名称、图像序号和文件扩展名。通过使用fmt % ...构造一个格式化字符串，然后使用.str()来获取生成的文件路径。

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

        double data[7] = {0};
        for (int i = 0; i < 7; i++) {
            fin >> data[i];
        }
        Eigen::Quaterniond q(data[6], data[3], data[4], data[5]);
        Eigen::Isometry3d T(q);
        T.pretranslate(Eigen::Vector3d(data[0], data[1], data[2]));
        poses.push_back(T);
    }

PCL PointCloud:

PCL PointCloud类型介绍_pcl::pointcloud-CSDN博客

PCL PointCloud 类型介绍
在 PCL 中，PointT 是基本的点的表示形式，包括 PointXYZ、PointXYZRGB、Normal 等，而 PointCloud 则是存储点集的容器。

PointCloud 被定义在 point_cloud 文件中。

一、成员变量
header：包含点云的信息，详见 PCLHeader 类型介绍。

points：保存点云的容器，类型为 std::vector<PointT>。

width：类型为uint32_t，表示点云宽度（如果组织为图像结构），即一行点云的数量。

height：类型为uint32_t，表示点云高度（如果组织为图像结构）。

​ 若为有序点云，height 可以大于 1，即多行点云，每行固定点云的宽度；若为无序点云，height 等于 1，即一行点云，此时 width 的数量即为点云的数量。

is_dense：bool 类型，若点云中的数据都是有限的（不包含 inf/NaN 这样的值），则为 true，否则为 false。

sensor_origin_ ：类型为Eigen::Vector4f，指定传感器的采集位姿，用的少，一般不用管。

sensor_orientation_ ：类型为Eigen::Quaternionf，指定传感器的采集位姿（方向），用的少，一般不用管。

mapping_ ()：类型为boost::shared_ptr<MsgFieldMap>，这是由 ROS 整合推动的。用户不需要访问映射。

以上成员变量，黑体加粗的比较重要，要了解它们的含义。

typedef pcl::PointXYZRGB PointT;//定义一个点
    typedef pcl::PointCloud<PointT> PointCloud;//存点

PointCloud::Ptr current(new PointCloud);
current->points.push_back(p);